Merge pull request #6 from artivis/separating_node_computation

Separating node from computation

CMakeLists.txt

@@ -21,26 +21,18 @@ find_package(catkin REQUIRED COMPONENTS
   tf  
 )
 
+set(MRPT_DONT_USE_DBG_LIBS 1)
+
 ## System dependencies are found with CMake's conventions
 find_package(Boost REQUIRED COMPONENTS system)
 find_package(cmake_modules REQUIRED)
 find_package(Eigen3 REQUIRED)
-
+find_package(MRPT REQUIRED base obs) # maps slam
-find_package(MRPT REQUIRED)
-MESSAGE(STATUS "Found MRPT: " ${MRPT_VERSION})
-IF(MRPT_VERSION VERSION_LESS 1.9.9)
-        # MRPT<2.0
-        find_package(MRPT REQUIRED base obs maps slam)
-ELSE()
-        # MRPT>=2.0
-        find_package(MRPT REQUIRED obs maps slam poses core)
-ENDIF()
-
 
 #include_directories(${MRPT_INCLUDE_DIRS})
-#MESSAGE( STATUS "MRPT_INCLUDE_DIRS: " ${MRPT_INCLUDE_DIRS})
+MESSAGE( STATUS "MRPT_INCLUDE_DIRS: " ${MRPT_INCLUDE_DIRS})
 #link_directories(${MRPT_LIBRARY_DIRS})
-#MESSAGE( STATUS "MRPT_LIBRARY_DIRS: " ${MRPT_LIBS})
+MESSAGE( STATUS "MRPT_LIBRARY_DIRS: " ${MRPT_LIBRARIES})
 
 
 
@@ -55,9 +47,9 @@ ENDIF()
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
  INCLUDE_DIRS include
- LIBRARIES laser_odometry
+ LIBRARIES ${PROJECT_NAME}
  CATKIN_DEPENDS nav_msgs roscpp sensor_msgs std_msgs tf
- #DEPENDS system_lib
+ DEPENDS MRPT
 )
 
 ## Specify additional locations of header files
@@ -65,19 +57,19 @@ catkin_package(
 include_directories(include)
 
 include_directories(
+  SYSTEM
   ${catkin_INCLUDE_DIRS}
   ${Boost_INCLUDE_DIRS}
   ${EIGEN_INCLUDE_DIRS}
   ${MRPT_INCLUDE_DIRS}
 )
 
-## Declare a cpp executable
+## Declare a cpp library
-add_executable(rf2o_laser_odometry_node src/CLaserOdometry2D.cpp)
+add_library(${PROJECT_NAME}
-
+    src/CLaserOdometry2D.cpp
-## Specify libraries to link a library or executable target against
-target_link_libraries(rf2o_laser_odometry_node
-   ${catkin_LIBRARIES}
-   ${Boost_LIBRARIES}
-   ${EIGEN_LIBRARIES}
-   ${MRPT_LIBS}
 )
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${MRPT_LIBS})
+
+## Declare a cpp executable
+add_executable(rf2o_laser_odometry_node src/CLaserOdometry2DNode.cpp)
+target_link_libraries(rf2o_laser_odometry_node ${PROJECT_NAME} ${catkin_LIBRARIES})

include/rf2o_laser_odometry/CLaserOdometry2D.h

@@ -17,24 +17,19 @@
 #define CLaserOdometry2D_H
 
 #include <ros/ros.h>
-#include <tf/transform_broadcaster.h>
-#include <tf/transform_listener.h>
 #include <nav_msgs/Odometry.h>
 #include <sensor_msgs/LaserScan.h>
-#include <geometry_msgs/Twist.h>
 
 // MRPT related headers
 #include <mrpt/version.h>
 #if MRPT_VERSION>=0x130
 #   include <mrpt/obs/CObservation2DRangeScan.h>
 #   include <mrpt/obs/CObservationOdometry.h>
-    #include <mrpt/utils/CTicTac.h>
+    typedef mrpt::obs::CObservation2DRangeScan CObservation2DRangeScan;
-    using namespace mrpt::obs;
 #else
 #   include <mrpt/slam/CObservation2DRangeScan.h>
 #   include <mrpt/slam/CObservationOdometry.h>
-    using namespace mrpt::slam;
+    typedef mrpt::poses::CObservation2DRangeScan CObservation2DRangeScan;
-    #include <mrpt/utils.h>
 #endif
 
 #if MRPT_VERSION<0x150
@@ -43,7 +38,8 @@
 
 #include <mrpt/system/os.h>
 #include <mrpt/poses/CPose3D.h>
-#include <mrpt/opengl.h>
+#include <mrpt/utils.h>
+//#include <mrpt/opengl.h>
 #include <mrpt/math/CHistogram.h>
 
 #include <boost/bind.hpp>
@@ -60,34 +56,26 @@ class CLaserOdometry2D
 public:
 	CLaserOdometry2D();
     ~CLaserOdometry2D();
-    bool is_initialized();
-    bool scan_available();
-    void Init();
-    void odometryCalculation();
 
-    std::string         laser_scan_topic;
+    void Init(const sensor_msgs::LaserScan& scan,
-    std::string         odom_topic;
+              const geometry_msgs::Pose& initial_robot_pose);
-    bool                publish_tf;
+
-    std::string         base_frame_id;
+    bool is_initialized();
-    std::string         odom_frame_id;
+
-    std::string         init_pose_from_topic;
+    void odometryCalculation(const sensor_msgs::LaserScan& scan);
-    double              freq;
+
+    void setLaserPose(const mrpt::poses::CPose3D& laser_pose);
+
+    const mrpt::poses::CPose3D& getIncrement() const;
+
+    const Eigen::Matrix<float, 3, 3>& getIncrementCovariance() const;
+
+    mrpt::poses::CPose3D& getPose();
+    const mrpt::poses::CPose3D& getPose() const;
 
 protected:
-    ros::NodeHandle             n;
-    sensor_msgs::LaserScan      last_scan;
-    bool                        module_initialized,first_laser_scan,new_scan_available, GT_pose_initialized, verbose;
-    tf::TransformListener       tf_listener;          //Do not put inside the callback
-    tf::TransformBroadcaster    odom_broadcaster;
-    nav_msgs::Odometry          initial_robot_pose;
 
-    //Subscriptions & Publishers
+  bool verbose,module_initialized,first_laser_scan;
-    ros::Subscriber laser_sub, initPose_sub;
-    ros::Publisher odom_pub;
-
-    //CallBacks
-    void LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan);
-    void initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose);
 
     // Internal Data
 	std::vector<Eigen::MatrixXf> range;
@@ -131,16 +119,23 @@ protected:
     unsigned int iter_irls;
 	float g_mask[5];
 
+  double lin_speed, ang_speed;
+
     //mrpt::gui::CDisplayWindowPlots window;
 	mrpt::utils::CTicTac		m_clock;
 	float		m_runtime;
-    ros::Time last_odom_time;
+    ros::Time last_odom_time, current_scan_time;
 
 	mrpt::math::CMatrixFloat31 kai_abs;
 	mrpt::math::CMatrixFloat31 kai_loc;
 	mrpt::math::CMatrixFloat31 kai_loc_old;
 	mrpt::math::CMatrixFloat31 kai_loc_level;
 
+  mrpt::poses::CPose3D last_increment;
+
+  mrpt::poses::CPose3D laser_pose_on_robot;
+  mrpt::poses::CPose3D laser_pose_on_robot_inv;
+
   mrpt::poses::CPose3D laser_pose;
 	mrpt::poses::CPose3D laser_oldpose;
     mrpt::poses::CPose3D robot_pose;

src/CLaserOdometry2D.cpp

@@ -14,119 +14,54 @@
 ******************************************************************************************** */
 
 #include "rf2o_laser_odometry/CLaserOdometry2D.h"
-using namespace mrpt;
-using namespace mrpt::math;
-using namespace mrpt::utils;
-using namespace mrpt::poses;
-using namespace std;
-using namespace Eigen;
-
 
 // --------------------------------------------
 // CLaserOdometry2D
 //---------------------------------------------
 
-CLaserOdometry2D::CLaserOdometry2D()
+CLaserOdometry2D::CLaserOdometry2D() :
+  module_initialized(false),
+  first_laser_scan(true)
 {
-    ROS_INFO("Initializing RF2O node...");
+  //
-
-    //Read Parameters
-    //----------------
-    ros::NodeHandle pn("~");
-    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>("base_frame_id", base_frame_id, "/base_link");
-    pn.param<std::string>("odom_frame_id", odom_frame_id, "/odom");
-    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<double>("freq",freq,10.0);
-    pn.param<bool>("verbose", verbose, true);
-
-    //Publishers and Subscribers
-    //--------------------------    
-    odom_pub = pn.advertise<nav_msgs::Odometry>(odom_topic, 5);
-    laser_sub = n.subscribe<sensor_msgs::LaserScan>(laser_scan_topic,1,&CLaserOdometry2D::LaserCallBack,this);
-
-    //init pose??
-    if (init_pose_from_topic != "")
-    {
-        initPose_sub = n.subscribe<nav_msgs::Odometry>(init_pose_from_topic,1,&CLaserOdometry2D::initPoseCallBack,this);
-        GT_pose_initialized  = false;
-    }
-    else
-    {
-        GT_pose_initialized = true;
-        initial_robot_pose.pose.pose.position.x = 0;
-        initial_robot_pose.pose.pose.position.y = 0;
-        initial_robot_pose.pose.pose.position.z = 0;
-        initial_robot_pose.pose.pose.orientation.w = 0;
-        initial_robot_pose.pose.pose.orientation.x = 0;
-        initial_robot_pose.pose.pose.orientation.y = 0;
-        initial_robot_pose.pose.pose.orientation.z = 0;
-    }
-
-    //Init variables
-    module_initialized = false;
-    first_laser_scan = true;
 }
 
 
 CLaserOdometry2D::~CLaserOdometry2D()
 {
+  //
 }
 
+void CLaserOdometry2D::setLaserPose(const mrpt::poses::CPose3D& laser_pose)
+{
+  //Set laser pose on the robot
+
+  laser_pose_on_robot = laser_pose;
+  laser_pose_on_robot_inv = laser_pose_on_robot;
+  laser_pose_on_robot_inv.inverse();
+}
 
 bool CLaserOdometry2D::is_initialized()
 {
     return module_initialized;
 }
 
-bool CLaserOdometry2D::scan_available()
+void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan,
-{
+                            const geometry_msgs::Pose& initial_robot_pose)
-    return new_scan_available;
-}
-
-void CLaserOdometry2D::Init()
 {
     //Got an initial scan laser, obtain its parametes
-    if (verbose)
+    ROS_INFO_COND(verbose, "[rf2o] Got first Laser Scan .... Configuring node");
-        ROS_INFO("[rf2o] Got first Laser Scan .... Configuring node");
+
-    width = last_scan.ranges.size();    // Num of samples (size) of the scan laser
+    width = scan.ranges.size();    // Num of samples (size) of the scan laser
+
     cols = width;						// Max reolution. Should be similar to the width parameter
-    fovh = fabs(last_scan.angle_max - last_scan.angle_min); // Horizontal Laser's FOV
+    fovh = fabs(scan.angle_max - scan.angle_min); // Horizontal Laser's FOV
     ctf_levels = 5;                     // Coarse-to-Fine levels
     iter_irls = 5;                      //Num iterations to solve iterative reweighted least squares
 
-    //Set laser pose on the robot (through tF)
-    // This allow estimation of the odometry with respect to the robot base reference system.
-    mrpt::poses::CPose3D LaserPoseOnTheRobot;
-    tf::StampedTransform transform;
-    try
-    {
-        tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform);
-    }
-    catch (tf::TransformException &ex)
-    {
-        ROS_ERROR("%s",ex.what());
-        ros::Duration(1.0).sleep();
-    }
-
-    //TF:transform -> mrpt::CPose3D (see mrpt-ros-bridge)
-    const tf::Vector3 &t = transform.getOrigin();
-    LaserPoseOnTheRobot.x() = t[0];
-    LaserPoseOnTheRobot.y() = t[1];
-    LaserPoseOnTheRobot.z() = t[2];
-    const tf::Matrix3x3 &basis = transform.getBasis();
-    mrpt::math::CMatrixDouble33 R;
-    for(int r = 0; r < 3; r++)
-        for(int c = 0; c < 3; c++)
-            R(r,c) = basis[r][c];
-    LaserPoseOnTheRobot.setRotationMatrix(R);
-
-
     //Robot initial pose (see MQTT:bridge)
     mrpt::poses::CPose3D robotInitialPose;
-    geometry_msgs::Pose _src = initial_robot_pose.pose.pose;
+    geometry_msgs::Pose _src = initial_robot_pose;
 
     robotInitialPose.x(_src.position.x);
     robotInitialPose.y(_src.position.y);
@@ -141,12 +76,9 @@ void CLaserOdometry2D::Init()
     robotInitialPose.setYawPitchRoll(yaw,pitch,roll);
     //robotInitialPose.phi(yaw);
 
-
-
     //Set the initial pose
-    laser_pose = robotInitialPose + LaserPoseOnTheRobot;
+    laser_pose = robotInitialPose + laser_pose_on_robot;
-    laser_oldpose = robotInitialPose + LaserPoseOnTheRobot;
+    laser_oldpose = robotInitialPose + laser_pose_on_robot;
-
 
     // Init module (internal)
     //------------------------
@@ -226,13 +158,36 @@ void CLaserOdometry2D::Init()
     last_odom_time = ros::Time::now();
 }
 
+const mrpt::poses::CPose3D& CLaserOdometry2D::getIncrement() const
+{
+  return last_increment;
+}
 
-void CLaserOdometry2D::odometryCalculation()
+const Eigen::Matrix<float, 3, 3>& CLaserOdometry2D::getIncrementCovariance() const
+{
+  return cov_odo;
+}
+
+mrpt::poses::CPose3D& CLaserOdometry2D::getPose()
+{
+  return robot_pose;
+}
+
+const mrpt::poses::CPose3D& CLaserOdometry2D::getPose() const
+{
+  return robot_pose;
+}
+
+void CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan)
 {
     //==================================================================================
     //						DIFERENTIAL  ODOMETRY  MULTILEVEL
     //==================================================================================
 
+    //copy laser scan to internal variable
+    for (unsigned int i = 0; i<width; i++)
+        range_wf(i) = scan.ranges[i];
+
     m_clock.Tic();
     createImagePyramid();
 
@@ -284,12 +239,11 @@ void CLaserOdometry2D::odometryCalculation()
     }
 
     m_runtime = 1000*m_clock.Tac();
-    if (verbose)
+
-        ROS_INFO("[rf2o] execution time (ms): %f", m_runtime);
+    ROS_INFO_COND(verbose, "[rf2o] execution time (ms): %f", m_runtime);
 
     //Update poses
     PoseUpdate();
-    new_scan_available = false;     //avoids the possibility to run twice on the same laser scan
 }
 
 
@@ -495,8 +449,8 @@ void CLaserOdometry2D::calculaterangeDerivativesSurface()
 
 	for (unsigned int u = 0; u < cols_i-1; u++)
     {
-		const float dist = square(xx_inter[image_level](u+1) - xx_inter[image_level](u))
+    const float dist = mrpt::math::square(xx_inter[image_level](u+1) - xx_inter[image_level](u))
-							+ square(yy_inter[image_level](u+1) - yy_inter[image_level](u));
+              + mrpt::math::square(yy_inter[image_level](u+1) - yy_inter[image_level](u));
 		if (dist  > 0.f)
 			rtita(u) = sqrt(dist);
 	}
@@ -572,7 +526,7 @@ void CLaserOdometry2D::computeWeights()
 
 	//Parameters for error_linearization
 	const float kdtita = 1.f;
-	const float kdt = kdtita/square(fps);
+  const float kdt = kdtita/mrpt::math::square(fps);
 	const float k2d = 0.2f;
 
 	for (unsigned int u = 1; u < cols_i-1; u++)
@@ -586,7 +540,7 @@ void CLaserOdometry2D::computeWeights()
 			const float dtitat = ini_dtita - final_dtita;
 			const float dtita2 = dtita(u+1) - dtita(u-1);
 
-			const float w_der = kdt*square(dt(u)) + kdtita*square(dtita(u)) + k2d*(abs(dtitat) + abs(dtita2));
+      const float w_der = kdt*mrpt::math::square(dt(u)) + kdtita*mrpt::math::square(dtita(u)) + k2d*(abs(dtitat) + abs(dtita2));
 
 			weights(u) = sqrt(1.f/w_der);
 		}
@@ -642,13 +596,13 @@ void CLaserOdometry2D::solveSystemOneLevel()
 		}
 
 	//Solve the linear system of equations using a minimum least squares method
-	MatrixXf AtA, AtB;
+  Eigen::MatrixXf AtA, AtB;
 	AtA.multiply_AtA(A);
 	AtB.multiply_AtB(A,B);
 	Var = AtA.ldlt().solve(AtB);
 
 	//Covariance matrix calculation 	Cov Order -> vx,vy,wz
-	MatrixXf res(num_valid_range,1);
+  Eigen::MatrixXf res(num_valid_range,1);
 	res = A*Var - B;
 	cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm();
 
@@ -684,13 +638,13 @@ void CLaserOdometry2D::solveSystemNonLinear()
         }
 
     //Solve the linear system of equations using a minimum least squares method
-    MatrixXf AtA, AtB;
+    Eigen::MatrixXf AtA, AtB;
     AtA.multiply_AtA(A);
     AtB.multiply_AtB(A,B);
     Var = AtA.ldlt().solve(AtB);
 
     //Covariance matrix calculation 	Cov Order -> vx,vy,wz
-    MatrixXf res(num_valid_range,1);
+    Eigen::MatrixXf res(num_valid_range,1);
     res = A*Var - B;
     //cout << endl << "max res: " << res.maxCoeff();
     //cout << endl << "min res: " << res.minCoeff();
@@ -711,7 +665,7 @@ void CLaserOdometry2D::solveSystemNonLinear()
     const float k = 10.f/aver_dt; //200
     //float energy = 0.f;
     //for (unsigned int i=0; i<res.rows(); i++)
-    //	energy += log(1.f + square(k*res(i)));
+    //	energy += log(1.f + mrpt::math::square(k*res(i)));
     //printf("\n\nEnergy(0) = %f", energy);
 
     //Solve iterative reweighted least squares
@@ -723,7 +677,7 @@ void CLaserOdometry2D::solveSystemNonLinear()
         for (unsigned int u = 1; u < cols_i-1; u++)
             if (null(u) == 0)
             {
-                const float res_weight = sqrt(1.f/(1.f + square(k*res(cont))));
+                const float res_weight = sqrt(1.f/(1.f + mrpt::math::square(k*res(cont))));
 
                 //Fill the matrix Aw
                 Aw(cont,0) = res_weight*A(cont,0);
@@ -742,17 +696,17 @@ void CLaserOdometry2D::solveSystemNonLinear()
         ////Compute the energy
         //energy = 0.f;
         //for (unsigned int j=0; j<res.rows(); j++)
-        //	energy += log(1.f + square(k*res(j)));
+        //	energy += log(1.f + mrpt::math::square(k*res(j)));
         //printf("\nEnergy(%d) = %f", i, energy);
     }
 
     cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm();
     kai_loc_level = Var;
-    if (verbose)
+
-        std::cout << endl << "[rf2o] COV_ODO: " << cov_odo  << endl;
+    ROS_INFO_STREAM_COND(verbose, "[rf2o] COV_ODO: " << cov_odo);
 }
 
-void CLaserOdometry2D::Reset(CPose3D ini_pose, CObservation2DRangeScan scan)
+void CLaserOdometry2D::Reset(mrpt::poses::CPose3D ini_pose, CObservation2DRangeScan scan)
 {
 	//Set the initial pose
 	laser_pose = ini_pose;
@@ -767,12 +721,13 @@ void CLaserOdometry2D::Reset(CPose3D ini_pose, CObservation2DRangeScan scan)
 
 void CLaserOdometry2D::performWarping()
 {
-	Matrix3f acu_trans; 
+  Eigen::Matrix3f acu_trans;
+
 	acu_trans.setIdentity();
 	for (unsigned int i=1; i<=level; i++)
 		acu_trans = transformations[i-1]*acu_trans;
 
-	MatrixXf wacu(1,cols_i);
+  Eigen::MatrixXf wacu(1,cols_i);
 	wacu.assign(0.f);
 	range_warped[image_level].assign(0.f);
 
@@ -808,11 +763,11 @@ void CLaserOdometry2D::performWarping()
 				}
 				else
 				{
-					const float w_r = square(delta_l);
+          const float w_r = mrpt::math::square(delta_l);
 					range_warped[image_level](uwarp_r) += w_r*range_w;
 					wacu(uwarp_r) += w_r;
 
-					const float w_l = square(delta_r);
+          const float w_l = mrpt::math::square(delta_r);
 					range_warped[image_level](uwarp_l) += w_l*range_w;
 					wacu(uwarp_l) += w_l;
 				}
@@ -847,28 +802,27 @@ void CLaserOdometry2D::filterLevelSolution()
 {
 	//		Calculate Eigenvalues and Eigenvectors
 	//----------------------------------------------------------
-	SelfAdjointEigenSolver<MatrixXf> eigensolver(cov_odo);
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigensolver(cov_odo);
-	if (eigensolver.info() != Success) 
+  if (eigensolver.info() != Eigen::Success)
 	{
-	    if (verbose)
+    ROS_INFO_COND(verbose, "[rf2o] ERROR: Eigensolver couldn't find a solution. Pose is not updated");
-    		printf("[rf2o] ERROR: Eigensolver couldn't find a solution. Pose is not updated");
 		return;
 	}
 
 	//First, we have to describe both the new linear and angular speeds in the "eigenvector" basis
 	//-------------------------------------------------------------------------------------------------
-	Matrix<float,3,3> Bii;
+  Eigen::Matrix<float,3,3> Bii;
-	Matrix<float,3,1> kai_b;
+  Eigen::Matrix<float,3,1> kai_b;
 	Bii = eigensolver.eigenvectors();
 
 	kai_b = Bii.colPivHouseholderQr().solve(kai_loc_level);
 
 	//Second, we have to describe both the old linear and angular speeds in the "eigenvector" basis too
 	//-------------------------------------------------------------------------------------------------
-	CMatrixFloat31 kai_loc_sub;
+  mrpt::math::CMatrixFloat31 kai_loc_sub;
 
 	//Important: we have to substract the solutions from previous levels
-	Matrix3f acu_trans;
+  Eigen::Matrix3f acu_trans;
 	acu_trans.setIdentity();
 	for (unsigned int i=0; i<level; i++)
 		acu_trans = transformations[i]*acu_trans;
@@ -878,16 +832,22 @@ void CLaserOdometry2D::filterLevelSolution()
 	if (acu_trans(0,0) > 1.f)
 		kai_loc_sub(2) = 0.f;
     else
-		kai_loc_sub(2) = -fps*acos(acu_trans(0,0))*sign(acu_trans(1,0));
+    {
+#if MRPT_VERSION>=0x130
+        kai_loc_sub(2) = -fps*acos(acu_trans(0,0))*mrpt::utils::sign(acu_trans(1,0));
+#else
+        kai_loc_sub(2) = -fps*acos(acu_trans(0,0))*mrpt::math::sign(acu_trans(1,0));
+#endif
+    }
 	kai_loc_sub += kai_loc_old;
 
-	Matrix<float,3,1> kai_b_old;
+  Eigen::Matrix<float,3,1> kai_b_old;
 	kai_b_old = Bii.colPivHouseholderQr().solve(kai_loc_sub);
 
 	//Filter speed
 	const float cf = 15e3f*expf(-int(level)), df = 0.05f*expf(-int(level));
 
-	Matrix<float,3,1> kai_b_fil;
+  Eigen::Matrix<float,3,1> kai_b_fil;
 	for (unsigned int i=0; i<3; i++)
 	{
 			kai_b_fil(i,0) = (kai_b(i,0) + (cf*eigensolver.eigenvalues()(i,0) + df)*kai_b_old(i,0))/(1.f + cf*eigensolver.eigenvalues()(i,0) + df);
@@ -895,7 +855,7 @@ void CLaserOdometry2D::filterLevelSolution()
 	}
 
 	//Transform filtered speed to local reference frame and compute transformation
-	Matrix<float,3,1> kai_loc_fil = Bii.inverse().colPivHouseholderQr().solve(kai_b_fil);
+  Eigen::Matrix<float,3,1> kai_loc_fil = Bii.inverse().colPivHouseholderQr().solve(kai_b_fil);
 
 	//transformation
 	const float incrx = kai_loc_fil(0)/fps;
@@ -914,7 +874,7 @@ void CLaserOdometry2D::PoseUpdate()
 {
 	//First, compute the overall transformation
 	//---------------------------------------------------
-	Matrix3f acu_trans;
+  Eigen::Matrix3f acu_trans;
 	acu_trans.setIdentity();
 	for (unsigned int i=1; i<=ctf_levels; i++)
 		acu_trans = transformations[i-1]*acu_trans;
@@ -927,8 +887,13 @@ void CLaserOdometry2D::PoseUpdate()
 	if (acu_trans(0,0) > 1.f)
 		kai_loc(2) = 0.f;
 	else
-		kai_loc(2) = fps*acos(acu_trans(0,0))*sign(acu_trans(1,0));
+    {
-
+#if MRPT_VERSION>=0x130
+    kai_loc(2) = fps*acos(acu_trans(0,0))*mrpt::utils::sign(acu_trans(1,0));
+#else
+    kai_loc(2) = fps*acos(acu_trans(0,0))*mrpt::math::sign(acu_trans(1,0));
+#endif
+    }
 	//cout << endl << "Arc cos (incr tita): " << kai_loc(2);
 
 	float phi = laser_pose.yaw();
@@ -941,9 +906,12 @@ void CLaserOdometry2D::PoseUpdate()
 	//						Update poses
 	//-------------------------------------------------------
 	laser_oldpose = laser_pose;
-	math::CMatrixDouble33 aux_acu = acu_trans;
+
-	poses::CPose2D pose_aux_2D(acu_trans(0,2), acu_trans(1,2), kai_loc(2)/fps);
+  mrpt::math::CMatrixDouble33 aux_acu = acu_trans;
-        laser_pose = laser_pose + CPose3D(pose_aux_2D);
+  mrpt::poses::CPose2D pose_aux_2D(acu_trans(0,2), acu_trans(1,2), kai_loc(2)/fps);
+        laser_pose = laser_pose + mrpt::poses::CPose3D(pose_aux_2D);
+
+  last_increment = pose_aux_2D;
 
 
 
@@ -954,56 +922,26 @@ void CLaserOdometry2D::PoseUpdate()
 	kai_loc_old(1) = -kai_abs(0)*sin(phi) + kai_abs(1)*cos(phi);
 	kai_loc_old(2) = kai_abs(2);
 
-    if (verbose)
+    ROS_INFO_COND(verbose, "[rf2o] LASERodom = [%f %f %f]",laser_pose.x(),laser_pose.y(),laser_pose.yaw());
-        ROS_INFO("[rf2o] LASERodom = [%f %f %f]",laser_pose.x(),laser_pose.y(),laser_pose.yaw());
-
-
-    // GET ROBOT POSE from LASER POSE
-    //------------------------------  
-    mrpt::poses::CPose3D LaserPoseOnTheRobot_inv;
-    tf::StampedTransform transform;
-    try
-    {
-        tf_listener.lookupTransform(last_scan.header.frame_id, base_frame_id, ros::Time(0), transform);
-    }
-    catch (tf::TransformException &ex)
-    {
-        ROS_ERROR("%s",ex.what());
-        ros::Duration(1.0).sleep();
-    }
-
-    //TF:transform -> mrpt::CPose3D (see mrpt-ros-bridge)
-    const tf::Vector3 &t = transform.getOrigin();
-    LaserPoseOnTheRobot_inv.x() = t[0];
-    LaserPoseOnTheRobot_inv.y() = t[1];
-    LaserPoseOnTheRobot_inv.z() = t[2];
-    const tf::Matrix3x3 &basis = transform.getBasis();
-    mrpt::math::CMatrixDouble33 R;
-    for(int r = 0; r < 3; r++)
-        for(int c = 0; c < 3; c++)
-            R(r,c) = basis[r][c];
-    LaserPoseOnTheRobot_inv.setRotationMatrix(R);
 
     //Compose Transformations
-    robot_pose = laser_pose + LaserPoseOnTheRobot_inv;
+    robot_pose = laser_pose + laser_pose_on_robot_inv;
-    if (verbose)
+    ROS_INFO_COND(verbose, "BASEodom = [%f %f %f]",robot_pose.x(),robot_pose.y(),robot_pose.yaw());
-        ROS_INFO("BASEodom = [%f %f %f]",robot_pose.x(),robot_pose.y(),robot_pose.yaw());
-
 
     // Estimate linear/angular speeds (mandatory for base_local_planner)
     // last_scan -> the last scan received
     // last_odom_time -> The time of the previous scan lasser used to estimate the pose
     //-------------------------------------------------------------------------------------
-    double time_inc_sec = (last_scan.header.stamp - last_odom_time).toSec();
+    double time_inc_sec = (current_scan_time - last_odom_time).toSec();
-    last_odom_time = last_scan.header.stamp;
+    last_odom_time = current_scan_time;
-    double lin_speed = acu_trans(0,2) / time_inc_sec;
+    lin_speed = acu_trans(0,2) / time_inc_sec;
-    //double lin_speed = sqrt( square(robot_oldpose.x()-robot_pose.x()) + square(robot_oldpose.y()-robot_pose.y()) )/time_inc_sec;
+    //double lin_speed = sqrt( mrpt::math::square(robot_oldpose.x()-robot_pose.x()) + mrpt::math::square(robot_oldpose.y()-robot_pose.y()) )/time_inc_sec;
     double ang_inc = robot_pose.yaw() - robot_oldpose.yaw();
     if (ang_inc > 3.14159)
         ang_inc -= 2*3.14159;
     if (ang_inc < -3.14159)
         ang_inc += 2*3.14159;
-    double ang_speed = ang_inc/time_inc_sec;
+    ang_speed = ang_inc/time_inc_sec;
     robot_oldpose = robot_pose;
 
     //filter speeds
@@ -1020,112 +958,4 @@ void CLaserOdometry2D::PoseUpdate()
     double sum2 = std::accumulate(last_m_ang_speeds.begin(), last_m_ang_speeds.end(), 0.0);
     ang_speed = sum2 / last_m_ang_speeds.size();
     */
-
-    //first, we'll publish the odometry over tf
-    //---------------------------------------
-    if (publish_tf)
-    {
-        //ROS_INFO("[rf2o] Publishing TF: [base_link] to [odom]");
-        geometry_msgs::TransformStamped odom_trans;
-        odom_trans.header.stamp = ros::Time::now();
-        odom_trans.header.frame_id = odom_frame_id;
-        odom_trans.child_frame_id = base_frame_id;
-        odom_trans.transform.translation.x = robot_pose.x();
-        odom_trans.transform.translation.y = robot_pose.y();
-        odom_trans.transform.translation.z = 0.0;
-        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(robot_pose.yaw());
-        //send the transform
-        odom_broadcaster.sendTransform(odom_trans);
-    }
-
-    //next, we'll publish the odometry message over ROS
-    //-------------------------------------------------
-    //ROS_INFO("[rf2o] Publishing Odom Topic");
-    nav_msgs::Odometry odom;
-    odom.header.stamp = ros::Time::now();
-    odom.header.frame_id = odom_frame_id;
-    //set the position
-    odom.pose.pose.position.x = robot_pose.x();
-    odom.pose.pose.position.y = robot_pose.y();
-    odom.pose.pose.position.z = 0.0;
-    odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(robot_pose.yaw());
-    //set the velocity
-    odom.child_frame_id = base_frame_id;
-    odom.twist.twist.linear.x = lin_speed;    //linear speed
-    odom.twist.twist.linear.y = 0.0;
-    odom.twist.twist.angular.z = ang_speed;   //angular speed
-    //publish the message
-    odom_pub.publish(odom);
-}
-
-
-
-//-----------------------------------------------------------------------------------
-//                                   CALLBACKS
-//-----------------------------------------------------------------------------------
-
-void CLaserOdometry2D::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan)
-{
-    if (GT_pose_initialized)
-    {
-        //Keep in memory the last received laser_scan
-        last_scan = *new_scan;
-
-        //Initialize module on first scan
-        if (first_laser_scan)
-        {
-            Init();
-            first_laser_scan = false;
-        }
-        else
-        {
-            //copy laser scan to internal variable
-            for (unsigned int i = 0; i<width; i++)
-                range_wf(i) = new_scan->ranges[i];
-            new_scan_available = true;
-        }
-    }
-}
-
-
-void CLaserOdometry2D::initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose)
-{
-    //Initialize module on first GT pose. Else do Nothing!
-    if (!GT_pose_initialized)
-    {
-        initial_robot_pose = *new_initPose;
-        GT_pose_initialized = true;
-    }
-}
-
-//-----------------------------------------------------------------------------------
-//                                   MAIN
-//-----------------------------------------------------------------------------------
-int main(int argc, char** argv)
-{
-    ros::init(argc, argv, "RF2O_LaserOdom");
-
-    CLaserOdometry2D myLaserOdom;
-
-    //Main Loop
-    //----------
-    ROS_INFO("[rf2o] initialization complete...Looping");
-    ros::Rate loop_rate(myLaserOdom.freq);
-    while (ros::ok())
-    {
-        ros::spinOnce();        //Check for new laser scans
-
-        if( myLaserOdom.is_initialized() && myLaserOdom.scan_available() )
-        {            
-            //Process odometry estimation
-            myLaserOdom.odometryCalculation();
-        }
-        else
-        {
-            ROS_WARN("[rf2o] Waiting for laser_scans....") ;
-        }
-
-        loop_rate.sleep();
-    }
-    return(0);
 }

src/CLaserOdometry2DNode.cpp

@@ -0,0 +1,275 @@
+/** ****************************************************************************************
+*  This node presents a fast and precise method to estimate the planar motion of a lidar
+*  from consecutive range scans. It is very useful for the estimation of the robot odometry from
+*  2D laser range measurements.
+*  This module is developed for mobile robots with innacurate or inexistent built-in odometry.
+*  It allows the estimation of a precise odometry with low computational cost.
+*  For more information, please refer to:
+*
+*  Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA'16.
+*  Available at: http://mapir.isa.uma.es/mapirwebsite/index.php/mapir-downloads/papers/217
+*
+* Maintainer: Javier G. Monroy
+* MAPIR group: http://mapir.isa.uma.es/
+******************************************************************************************** */
+
+#include "rf2o_laser_odometry/CLaserOdometry2D.h"
+
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_listener.h>
+
+class CLaserOdometry2DNode : CLaserOdometry2D
+{
+public:
+
+  CLaserOdometry2DNode();
+  ~CLaserOdometry2DNode();
+
+  void process(const ros::TimerEvent &);
+  void publish();
+
+  bool setLaserPoseFromTf();
+
+public:
+
+  bool publish_tf,new_scan_available;
+
+  double freq;
+
+  std::string         laser_scan_topic;
+  std::string         odom_topic;
+  std::string         base_frame_id;
+  std::string         odom_frame_id;
+  std::string         init_pose_from_topic;
+
+  ros::NodeHandle             n;
+  sensor_msgs::LaserScan      last_scan;
+  bool                        GT_pose_initialized;
+  tf::TransformListener       tf_listener;          //Do not put inside the callback
+  tf::TransformBroadcaster    odom_broadcaster;
+  nav_msgs::Odometry          initial_robot_pose;
+
+  //Subscriptions & Publishers
+  ros::Subscriber laser_sub, initPose_sub;
+  ros::Publisher odom_pub;
+
+  bool scan_available();
+
+  //CallBacks
+  void LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan);
+  void initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose);
+};
+
+CLaserOdometry2DNode::CLaserOdometry2DNode() :
+  CLaserOdometry2D()
+{
+    ROS_INFO("Initializing RF2O node...");
+
+    //Read Parameters
+    //----------------
+    ros::NodeHandle pn("~");
+    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>("base_frame_id", base_frame_id, "/base_link");
+    pn.param<std::string>("odom_frame_id", odom_frame_id, "/odom");
+    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<double>("freq",freq,10.0);
+    pn.param<bool>("verbose", verbose, true);
+
+    //Publishers and Subscribers
+    //--------------------------
+    odom_pub  = pn.advertise<nav_msgs::Odometry>(odom_topic, 5);
+    laser_sub = n.subscribe<sensor_msgs::LaserScan>(laser_scan_topic,1,&CLaserOdometry2DNode::LaserCallBack,this);
+
+    //init pose??
+    if (init_pose_from_topic != "")
+    {
+        initPose_sub = n.subscribe<nav_msgs::Odometry>(init_pose_from_topic,1,&CLaserOdometry2DNode::initPoseCallBack,this);
+        GT_pose_initialized  = false;
+    }
+    else
+    {
+        GT_pose_initialized = true;
+        initial_robot_pose.pose.pose.position.x = 0;
+        initial_robot_pose.pose.pose.position.y = 0;
+        initial_robot_pose.pose.pose.position.z = 0;
+        initial_robot_pose.pose.pose.orientation.w = 0;
+        initial_robot_pose.pose.pose.orientation.x = 0;
+        initial_robot_pose.pose.pose.orientation.y = 0;
+        initial_robot_pose.pose.pose.orientation.z = 0;
+    }
+
+    setLaserPoseFromTf();
+
+    //Init variables
+    module_initialized = false;
+    first_laser_scan   = true;
+
+  ROS_INFO_STREAM("Listening laser scan from topic: " << laser_sub.getTopic());
+}
+
+CLaserOdometry2DNode::~CLaserOdometry2DNode()
+{
+  //
+}
+
+bool CLaserOdometry2DNode::setLaserPoseFromTf()
+{
+  bool retrieved = false;
+
+  // Set laser pose on the robot (through tF)
+  // This allow estimation of the odometry with respect to the robot base reference system.
+  tf::StampedTransform transform;
+  transform.setIdentity();
+  try
+  {
+      tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform);
+      retrieved = true;
+  }
+  catch (tf::TransformException &ex)
+  {
+      ROS_ERROR("%s",ex.what());
+      ros::Duration(1.0).sleep();
+      retrieved = false;
+  }
+
+  //TF:transform -> mrpt::CPose3D (see mrpt-ros-bridge)
+  mrpt::poses::CPose3D laser_tf;
+  const tf::Vector3 &t = transform.getOrigin();
+  laser_tf.x() = t[0];
+  laser_tf.y() = t[1];
+  laser_tf.z() = t[2];
+  const tf::Matrix3x3 &basis = transform.getBasis();
+  mrpt::math::CMatrixDouble33 R;
+  for(int r = 0; r < 3; r++)
+      for(int c = 0; c < 3; c++)
+          R(r,c) = basis[r][c];
+  laser_tf.setRotationMatrix(R);
+
+  setLaserPose(laser_tf);
+
+  return retrieved;
+}
+
+bool CLaserOdometry2DNode::scan_available()
+{
+    return new_scan_available;
+}
+
+void CLaserOdometry2DNode::process(const ros::TimerEvent&)
+{
+  if( is_initialized() && scan_available() )
+  {
+      //Process odometry estimation
+      odometryCalculation(last_scan);
+      publish();
+      new_scan_available = false; //avoids the possibility to run twice on the same laser scan
+  }
+  else
+  {
+      ROS_WARN("Waiting for laser_scans....") ;
+  }
+}
+
+//-----------------------------------------------------------------------------------
+//                                   CALLBACKS
+//-----------------------------------------------------------------------------------
+
+void CLaserOdometry2DNode::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan)
+{
+    if (GT_pose_initialized)
+    {
+        //Keep in memory the last received laser_scan
+        last_scan = *new_scan;
+        current_scan_time = last_scan.header.stamp;
+
+        //Initialize module on first scan
+        if (!first_laser_scan)
+        {
+            //copy laser scan to internal variable
+            for (unsigned int i = 0; i<width; i++)
+                range_wf(i) = new_scan->ranges[i];
+            new_scan_available = true;
+        }
+        else
+        {
+          Init(last_scan, initial_robot_pose.pose.pose);
+          first_laser_scan = false;
+        }
+    }
+}
+
+void CLaserOdometry2DNode::initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose)
+{
+    //Initialize module on first GT pose. Else do Nothing!
+    if (!GT_pose_initialized)
+    {
+        initial_robot_pose = *new_initPose;
+        GT_pose_initialized = true;
+    }
+}
+
+void CLaserOdometry2DNode::publish()
+{
+  //first, we'll publish the odometry over tf
+  //---------------------------------------
+  if (publish_tf)
+  {
+      //ROS_INFO("[rf2o] Publishing TF: [base_link] to [odom]");
+      geometry_msgs::TransformStamped odom_trans;
+      odom_trans.header.stamp = ros::Time::now();
+      odom_trans.header.frame_id = odom_frame_id;
+      odom_trans.child_frame_id = base_frame_id;
+      odom_trans.transform.translation.x = robot_pose.x();
+      odom_trans.transform.translation.y = robot_pose.y();
+      odom_trans.transform.translation.z = 0.0;
+      odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(robot_pose.yaw());
+      //send the transform
+      odom_broadcaster.sendTransform(odom_trans);
+  }
+
+  //next, we'll publish the odometry message over ROS
+  //-------------------------------------------------
+  //ROS_INFO("[rf2o] Publishing Odom Topic");
+  nav_msgs::Odometry odom;
+  odom.header.stamp = ros::Time::now();
+  odom.header.frame_id = odom_frame_id;
+  //set the position
+  odom.pose.pose.position.x = robot_pose.x();
+  odom.pose.pose.position.y = robot_pose.y();
+  odom.pose.pose.position.z = 0.0;
+  odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(robot_pose.yaw());
+  //set the velocity
+  odom.child_frame_id = base_frame_id;
+  odom.twist.twist.linear.x = lin_speed;    //linear speed
+  odom.twist.twist.linear.y = 0.0;
+  odom.twist.twist.angular.z = ang_speed;   //angular speed
+  //publish the message
+  odom_pub.publish(odom);
+}
+
+//-----------------------------------------------------------------------------------
+//                                   MAIN
+//-----------------------------------------------------------------------------------
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "RF2O_LaserOdom");
+
+    CLaserOdometry2DNode myLaserOdomNode;
+
+    ros::TimerOptions timer_opt;
+    timer_opt.oneshot   = false;
+    timer_opt.autostart = true;
+    timer_opt.callback_queue = ros::getGlobalCallbackQueue();
+    timer_opt.tracked_object = ros::VoidConstPtr();
+
+    timer_opt.callback = boost::bind(&CLaserOdometry2DNode::process, &myLaserOdomNode, _1);
+    timer_opt.period   = ros::Rate(myLaserOdomNode.freq).expectedCycleTime();
+
+    ros::Timer rf2o_timer = ros::NodeHandle("~").createTimer(timer_opt);
+
+    ros::spin();
+
+    return EXIT_SUCCESS;
+}