mirrors/rf2o_laser_odometry
1
0
Fork 0
mirror of https://github.com/MAPIRlab/rf2o_laser_odometry.git synced 2023-04-06 19:28:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix indentation + credits

Browse Source
This commit is contained in:
Jeremie Deray
2017-08-16 10:03:13 +02:00
parent f159d3eb2e
commit 77fb8e3a86
3 changed files with 779 additions and 789 deletions
Download Patch File Download Diff File Expand all files Collapse all files

127
include/rf2o_laser_odometry/CLaserOdometry2D.h
View File

@@ -11,6 +11,8 @@
* *
* Maintainer: Javier G. Monroy * Maintainer: Javier G. Monroy
* MAPIR group: http://mapir.isa.uma.es/ * MAPIR group: http://mapir.isa.uma.es/
*
* Modifications: Jeremie Deray
******************************************************************************************** */ ******************************************************************************************** */
#ifndef CLaserOdometry2D_H #ifndef CLaserOdometry2D_H
@@ -72,74 +74,74 @@ public:
using MatrixS31 = Eigen::Matrix<Scalar, 3, 1>; using MatrixS31 = Eigen::Matrix<Scalar, 3, 1>;
using IncrementCov = Eigen::Matrix<Scalar, 3, 3>; using IncrementCov = Eigen::Matrix<Scalar, 3, 3>;
CLaserOdometry2D(); CLaserOdometry2D();
virtual ~CLaserOdometry2D() = default; virtual ~CLaserOdometry2D() = default;
void init(const sensor_msgs::LaserScan& scan, void init(const sensor_msgs::LaserScan& scan,
const geometry_msgs::Pose& initial_robot_pose); const geometry_msgs::Pose& initial_robot_pose);
bool is_initialized(); bool is_initialized();
bool odometryCalculation(const sensor_msgs::LaserScan& scan); bool odometryCalculation(const sensor_msgs::LaserScan& scan);
void setLaserPose(const Pose3d& laser_pose); void setLaserPose(const Pose3d& laser_pose);
const Pose3d& getIncrement() const; const Pose3d& getIncrement() const;
const IncrementCov& getIncrementCovariance() const; const IncrementCov& getIncrementCovariance() const;
Pose3d& getPose(); Pose3d& getPose();
const Pose3d& getPose() const; const Pose3d& getPose() const;
protected: protected:
bool verbose, module_initialized, first_laser_scan; bool verbose, module_initialized, first_laser_scan;
// Internal Data // Internal Data
std::vector<Eigen::MatrixXf> range; std::vector<Eigen::MatrixXf> range;
std::vector<Eigen::MatrixXf> range_old; std::vector<Eigen::MatrixXf> range_old;
std::vector<Eigen::MatrixXf> range_inter; std::vector<Eigen::MatrixXf> range_inter;
std::vector<Eigen::MatrixXf> range_warped; std::vector<Eigen::MatrixXf> range_warped;
std::vector<Eigen::MatrixXf> xx; std::vector<Eigen::MatrixXf> xx;
std::vector<Eigen::MatrixXf> xx_inter; std::vector<Eigen::MatrixXf> xx_inter;
std::vector<Eigen::MatrixXf> xx_old; std::vector<Eigen::MatrixXf> xx_old;
std::vector<Eigen::MatrixXf> xx_warped; std::vector<Eigen::MatrixXf> xx_warped;
std::vector<Eigen::MatrixXf> yy; std::vector<Eigen::MatrixXf> yy;
std::vector<Eigen::MatrixXf> yy_inter; std::vector<Eigen::MatrixXf> yy_inter;
std::vector<Eigen::MatrixXf> yy_old; std::vector<Eigen::MatrixXf> yy_old;
std::vector<Eigen::MatrixXf> yy_warped; std::vector<Eigen::MatrixXf> yy_warped;
std::vector<Eigen::MatrixXf> transformations; std::vector<Eigen::MatrixXf> transformations;
Eigen::MatrixXf range_wf; Eigen::MatrixXf range_wf;
Eigen::MatrixXf dtita; Eigen::MatrixXf dtita;
Eigen::MatrixXf dt; Eigen::MatrixXf dt;
Eigen::MatrixXf rtita; Eigen::MatrixXf rtita;
Eigen::MatrixXf normx, normy, norm_ang; Eigen::MatrixXf normx, normy, norm_ang;
Eigen::MatrixXf weights; Eigen::MatrixXf weights;
Eigen::MatrixXi null; Eigen::MatrixXi null;
Eigen::MatrixXf A,Aw; Eigen::MatrixXf A,Aw;
Eigen::MatrixXf B,Bw; Eigen::MatrixXf B,Bw;
MatrixS31 Var; //3 unknowns: vx, vy, w MatrixS31 Var; //3 unknowns: vx, vy, w
IncrementCov cov_odo; IncrementCov cov_odo;
//std::string LaserVarName; //Name of the topic containing the scan lasers \laser_scan //std::string LaserVarName; //Name of the topic containing the scan lasers \laser_scan
float fps; //In Hz float fps; //In Hz
float fovh; //Horizontal FOV float fovh; //Horizontal FOV
unsigned int cols; unsigned int cols;
unsigned int cols_i; unsigned int cols_i;
unsigned int width; unsigned int width;
unsigned int ctf_levels; unsigned int ctf_levels;
unsigned int image_level, level; unsigned int image_level, level;
unsigned int num_valid_range; unsigned int num_valid_range;
unsigned int iter_irls; unsigned int iter_irls;
float g_mask[5]; float g_mask[5];
double lin_speed, ang_speed; double lin_speed, ang_speed;
ros::WallDuration m_runtime; ros::WallDuration m_runtime;
ros::Time last_odom_time, current_scan_time; ros::Time last_odom_time, current_scan_time;
MatrixS31 kai_abs_; MatrixS31 kai_abs_;
MatrixS31 kai_loc_; MatrixS31 kai_loc_;
@@ -154,24 +156,23 @@ protected:
Pose3d robot_pose_; Pose3d robot_pose_;
Pose3d robot_oldpose_; Pose3d robot_oldpose_;
bool test; bool test;
std::vector<double> last_m_lin_speeds; std::vector<double> last_m_lin_speeds;
std::vector<double> last_m_ang_speeds; std::vector<double> last_m_ang_speeds;
// Methods
// Methods void createImagePyramid();
void createImagePyramid(); void calculateCoord();
void calculateCoord(); void performWarping();
void performWarping(); void calculaterangeDerivativesSurface();
void calculaterangeDerivativesSurface(); void computeNormals();
void computeNormals(); void computeWeights();
void computeWeights(); void findNullPoints();
void findNullPoints(); void solveSystemOneLevel();
void solveSystemOneLevel(); void solveSystemNonLinear();
void solveSystemNonLinear(); void filterLevelSolution();
void filterLevelSolution(); void PoseUpdate();
void PoseUpdate(); void Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan scan*/);
void Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan scan*/);
}; };
#endif // CLaserOdometry2D_H #endif // CLaserOdometry2D_H

1245
src/CLaserOdometry2D.cpp
View File

File diff suppressed because it is too large Load Diff

196
src/CLaserOdometry2DNode.cpp
View File

@@ -11,6 +11,8 @@
* *
* Maintainer: Javier G. Monroy * Maintainer: Javier G. Monroy
* MAPIR group: http://mapir.isa.uma.es/ * MAPIR group: http://mapir.isa.uma.es/
*
* Modifications: Jeremie Deray
******************************************************************************************** */ ******************************************************************************************** */
#include "rf2o_laser_odometry/CLaserOdometry2D.h" #include "rf2o_laser_odometry/CLaserOdometry2D.h"
@@ -63,48 +65,48 @@ public:
CLaserOdometry2DNode::CLaserOdometry2DNode() : CLaserOdometry2DNode::CLaserOdometry2DNode() :
CLaserOdometry2D() CLaserOdometry2D()
{ {
ROS_INFO("Initializing RF2O node..."); ROS_INFO("Initializing RF2O node...");
//Read Parameters //Read Parameters
//---------------- //----------------
ros::NodeHandle pn("~"); ros::NodeHandle pn("~");
pn.param<std::string>("laser_scan_topic",laser_scan_topic,"/laser_scan"); pn.param<std::string>("laser_scan_topic",laser_scan_topic,"/laser_scan");
pn.param<std::string>("odom_topic", odom_topic, "/odom_rf2o"); pn.param<std::string>("odom_topic", odom_topic, "/odom_rf2o");
pn.param<std::string>("base_frame_id", base_frame_id, "/base_link"); pn.param<std::string>("base_frame_id", base_frame_id, "/base_link");
pn.param<std::string>("odom_frame_id", odom_frame_id, "/odom"); pn.param<std::string>("odom_frame_id", odom_frame_id, "/odom");
pn.param<bool>("publish_tf", publish_tf, true); pn.param<bool>("publish_tf", publish_tf, true);
pn.param<std::string>("init_pose_from_topic", init_pose_from_topic, "/base_pose_ground_truth"); pn.param<std::string>("init_pose_from_topic", init_pose_from_topic, "/base_pose_ground_truth");
pn.param<double>("freq",freq,10.0); pn.param<double>("freq",freq,10.0);
pn.param<bool>("verbose", verbose, true); pn.param<bool>("verbose", verbose, true);
//Publishers and Subscribers //Publishers and Subscribers
//-------------------------- //--------------------------
odom_pub = pn.advertise<nav_msgs::Odometry>(odom_topic, 5); odom_pub = pn.advertise<nav_msgs::Odometry>(odom_topic, 5);
laser_sub = n.subscribe<sensor_msgs::LaserScan>(laser_scan_topic,1,&CLaserOdometry2DNode::LaserCallBack,this); laser_sub = n.subscribe<sensor_msgs::LaserScan>(laser_scan_topic,1,&CLaserOdometry2DNode::LaserCallBack,this);
//init pose?? //init pose??
if (init_pose_from_topic != "") if (init_pose_from_topic != "")
{ {
initPose_sub = n.subscribe<nav_msgs::Odometry>(init_pose_from_topic,1,&CLaserOdometry2DNode::initPoseCallBack,this); initPose_sub = n.subscribe<nav_msgs::Odometry>(init_pose_from_topic,1,&CLaserOdometry2DNode::initPoseCallBack,this);
GT_pose_initialized = false; GT_pose_initialized = false;
} }
else else
{ {
GT_pose_initialized = true; GT_pose_initialized = true;
initial_robot_pose.pose.pose.position.x = 0; initial_robot_pose.pose.pose.position.x = 0;
initial_robot_pose.pose.pose.position.y = 0; initial_robot_pose.pose.pose.position.y = 0;
initial_robot_pose.pose.pose.position.z = 0; initial_robot_pose.pose.pose.position.z = 0;
initial_robot_pose.pose.pose.orientation.w = 0; initial_robot_pose.pose.pose.orientation.w = 0;
initial_robot_pose.pose.pose.orientation.x = 0; initial_robot_pose.pose.pose.orientation.x = 0;
initial_robot_pose.pose.pose.orientation.y = 0; initial_robot_pose.pose.pose.orientation.y = 0;
initial_robot_pose.pose.pose.orientation.z = 0; initial_robot_pose.pose.pose.orientation.z = 0;
} }
setLaserPoseFromTf(); setLaserPoseFromTf();
//Init variables //Init variables
module_initialized = false; module_initialized = false;
first_laser_scan = true; first_laser_scan = true;
ROS_INFO_STREAM("Listening laser scan from topic: " << laser_sub.getTopic()); ROS_INFO_STREAM("Listening laser scan from topic: " << laser_sub.getTopic());
} }
@@ -119,14 +121,14 @@ bool CLaserOdometry2DNode::setLaserPoseFromTf()
transform.setIdentity(); transform.setIdentity();
try try
{ {
tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform); tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform);
retrieved = true; retrieved = true;
} }
catch (tf::TransformException &ex) catch (tf::TransformException &ex)
{ {
ROS_ERROR("%s",ex.what()); ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep(); ros::Duration(1.0).sleep();
retrieved = false; retrieved = false;
} }
//TF:transform -> Eigen::Isometry3d //TF:transform -> Eigen::Isometry3d
@@ -135,8 +137,8 @@ bool CLaserOdometry2DNode::setLaserPoseFromTf()
Eigen::Matrix3d R; Eigen::Matrix3d R;
for(int r = 0; r < 3; r++) for(int r = 0; r < 3; r++)
for(int c = 0; c < 3; c++) for(int c = 0; c < 3; c++)
R(r,c) = basis[r][c]; R(r,c) = basis[r][c];
Pose3d laser_tf(R); Pose3d laser_tf(R);
@@ -152,21 +154,21 @@ bool CLaserOdometry2DNode::setLaserPoseFromTf()
bool CLaserOdometry2DNode::scan_available() bool CLaserOdometry2DNode::scan_available()
{ {
return new_scan_available; return new_scan_available;
} }
void CLaserOdometry2DNode::process(const ros::TimerEvent&) void CLaserOdometry2DNode::process(const ros::TimerEvent&)
{ {
if( is_initialized() && scan_available() ) if( is_initialized() && scan_available() )
{ {
//Process odometry estimation //Process odometry estimation
odometryCalculation(last_scan); odometryCalculation(last_scan);
publish(); publish();
new_scan_available = false; //avoids the possibility to run twice on the same laser scan new_scan_available = false; //avoids the possibility to run twice on the same laser scan
} }
else else
{ {
ROS_WARN("Waiting for laser_scans....") ; ROS_WARN("Waiting for laser_scans....") ;
} }
} }
@@ -176,36 +178,36 @@ void CLaserOdometry2DNode::process(const ros::TimerEvent&)
void CLaserOdometry2DNode::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan) void CLaserOdometry2DNode::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan)
{ {
if (GT_pose_initialized) if (GT_pose_initialized)
{ {
//Keep in memory the last received laser_scan //Keep in memory the last received laser_scan
last_scan = *new_scan; last_scan = *new_scan;
current_scan_time = last_scan.header.stamp; current_scan_time = last_scan.header.stamp;
//Initialize module on first scan //Initialize module on first scan
if (!first_laser_scan) if (!first_laser_scan)
{ {
//copy laser scan to internal variable //copy laser scan to internal variable
for (unsigned int i = 0; i<width; i++) for (unsigned int i = 0; i<width; i++)
range_wf(i) = new_scan->ranges[i]; range_wf(i) = new_scan->ranges[i];
new_scan_available = true; new_scan_available = true;
}
else
{
init(last_scan, initial_robot_pose.pose.pose);
first_laser_scan = false;
}
} }
else
{
init(last_scan, initial_robot_pose.pose.pose);
first_laser_scan = false;
}
}
} }
void CLaserOdometry2DNode::initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose) void CLaserOdometry2DNode::initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose)
{ {
//Initialize module on first GT pose. Else do Nothing! //Initialize module on first GT pose. Else do Nothing!
if (!GT_pose_initialized) if (!GT_pose_initialized)
{ {
initial_robot_pose = *new_initPose; initial_robot_pose = *new_initPose;
GT_pose_initialized = true; GT_pose_initialized = true;
} }
} }
void CLaserOdometry2DNode::publish() void CLaserOdometry2DNode::publish()
@@ -214,17 +216,17 @@ void CLaserOdometry2DNode::publish()
//--------------------------------------- //---------------------------------------
if (publish_tf) if (publish_tf)
{ {
//ROS_INFO("[rf2o] Publishing TF: [base_link] to [odom]"); //ROS_INFO("[rf2o] Publishing TF: [base_link] to [odom]");
geometry_msgs::TransformStamped odom_trans; geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = ros::Time::now(); odom_trans.header.stamp = ros::Time::now();
odom_trans.header.frame_id = odom_frame_id; odom_trans.header.frame_id = odom_frame_id;
odom_trans.child_frame_id = base_frame_id; odom_trans.child_frame_id = base_frame_id;
odom_trans.transform.translation.x = robot_pose_.translation()(0); odom_trans.transform.translation.x = robot_pose_.translation()(0);
odom_trans.transform.translation.y = robot_pose_.translation()(1); odom_trans.transform.translation.y = robot_pose_.translation()(1);
odom_trans.transform.translation.z = 0.0; odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(rf2o::getYaw(robot_pose_.rotation())); odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(rf2o::getYaw(robot_pose_.rotation()));
//send the transform //send the transform
odom_broadcaster.sendTransform(odom_trans); odom_broadcaster.sendTransform(odom_trans);
} }
//next, we'll publish the odometry message over ROS //next, we'll publish the odometry message over ROS
@@ -252,22 +254,22 @@ void CLaserOdometry2DNode::publish()
//----------------------------------------------------------------------------------- //-----------------------------------------------------------------------------------
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
ros::init(argc, argv, "RF2O_LaserOdom"); ros::init(argc, argv, "RF2O_LaserOdom");
CLaserOdometry2DNode myLaserOdomNode; CLaserOdometry2DNode myLaserOdomNode;
ros::TimerOptions timer_opt; ros::TimerOptions timer_opt;
timer_opt.oneshot = false; timer_opt.oneshot = false;
timer_opt.autostart = true; timer_opt.autostart = true;
timer_opt.callback_queue = ros::getGlobalCallbackQueue(); timer_opt.callback_queue = ros::getGlobalCallbackQueue();
timer_opt.tracked_object = ros::VoidConstPtr(); timer_opt.tracked_object = ros::VoidConstPtr();
timer_opt.callback = boost::bind(&CLaserOdometry2DNode::process, &myLaserOdomNode, _1); timer_opt.callback = boost::bind(&CLaserOdometry2DNode::process, &myLaserOdomNode, _1);
timer_opt.period = ros::Rate(myLaserOdomNode.freq).expectedCycleTime(); timer_opt.period = ros::Rate(myLaserOdomNode.freq).expectedCycleTime();
ros::Timer rf2o_timer = ros::NodeHandle("~").createTimer(timer_opt); ros::Timer rf2o_timer = ros::NodeHandle("~").createTimer(timer_opt);
ros::spin(); ros::spin();
return EXIT_SUCCESS; return EXIT_SUCCESS;
} }