Merge pull request #10 from artivis/mrpt_free

[WIP] Remove MRPT dependency
2023-04-06 19:28:55 +08:00 · 2018-11-21 13:17:48 +01:00
parent 06301fd315 1bc891bf58
commit d09068cfff
6 changed files with 1020 additions and 993 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -19,22 +19,12 @@ find_package(catkin REQUIRED COMPONENTS
  sensor_msgs
  std_msgs
  tf
+  cmake_modules
 )

-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
-
-#include_directories(${MRPT_INCLUDE_DIRS})
-MESSAGE( STATUS "MRPT_INCLUDE_DIRS: " ${MRPT_INCLUDE_DIRS})
-#link_directories(${MRPT_LIBRARY_DIRS})
-MESSAGE( STATUS "MRPT_LIBRARY_DIRS: " ${MRPT_LIBRARIES})
-
-

 ###################################
 ## catkin specific configuration ##
@@ -46,10 +36,10 @@ MESSAGE( STATUS "MRPT_LIBRARY_DIRS: " ${MRPT_LIBRARIES})
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
- INCLUDE_DIRS include
+ INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS}
 LIBRARIES ${PROJECT_NAME}
 CATKIN_DEPENDS nav_msgs roscpp sensor_msgs std_msgs tf
- DEPENDS MRPT
+ DEPENDS #Eigen3
 )

 ## Specify additional locations of header files
@@ -60,15 +50,12 @@ include_directories(
  SYSTEM
  ${catkin_INCLUDE_DIRS}
  ${Boost_INCLUDE_DIRS}
-  ${EIGEN_INCLUDE_DIRS}
-  ${MRPT_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
 )

 ## Declare a cpp library
-add_library(${PROJECT_NAME}
-    src/CLaserOdometry2D.cpp
-)
-target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${MRPT_LIBS})
+add_library(${PROJECT_NAME} src/CLaserOdometry2D.cpp)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})

 ## Declare a cpp executable
 add_executable(rf2o_laser_odometry_node src/CLaserOdometry2DNode.cpp)
--- a/README.md
+++ b/README.md
@@ -6,8 +6,3 @@ RF2O is a fast and precise method to estimate the planar motion of a lidar from
 Its very low computational cost (0.9 milliseconds on a single CPU core) together whit its high precission, makes RF2O a suitable method for those robotic applications that require planar odometry.

 For full description of the algorithm, please refer to: **Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA 2016** Available at: http://mapir.isa.uma.es/work/rf2o
-
-
-# Requirements
-RF2O core is implemented within the **Mobile Robot Programming Toolkit** [MRPT](http://www.mrpt.org/), so it is necessary to install this powerful library (see instructions [here](http://www.mrpt.org/download-mrpt/))
-So far RF2O has been tested with the Ubuntu official repository version (MRPT v1.3), and we are working to update it to MRPT v.1.9
--- a/include/rf2o_laser_odometry/CLaserOdometry2D.h
+++ b/include/rf2o_laser_odometry/CLaserOdometry2D.h
@@ -11,153 +11,170 @@
 *
 * Maintainer: Javier G. Monroy
 * MAPIR group: http://mapir.isa.uma.es/
+*
+* Modifications: Jeremie Deray
 ******************************************************************************************** */

 #ifndef CLaserOdometry2D_H
 #define CLaserOdometry2D_H

-#include <ros/ros.h>
-#include <nav_msgs/Odometry.h>
-#include <sensor_msgs/LaserScan.h>
-
-// MRPT related headers
-#include <mrpt/version.h>
-#if MRPT_VERSION>=0x130
-#   include <mrpt/obs/CObservation2DRangeScan.h>
-#   include <mrpt/obs/CObservationOdometry.h>
-    typedef mrpt::obs::CObservation2DRangeScan CObservation2DRangeScan;
-#else
-#   include <mrpt/slam/CObservation2DRangeScan.h>
-#   include <mrpt/slam/CObservationOdometry.h>
-    typedef mrpt::poses::CObservation2DRangeScan CObservation2DRangeScan;
-#endif
-
-#if MRPT_VERSION<0x150
-#   include <mrpt/system/threads.h>
-#endif
-
-#include <mrpt/system/os.h>
-#include <mrpt/poses/CPose3D.h>
-#include <mrpt/utils.h>
-//#include <mrpt/opengl.h>
-#include <mrpt/math/CHistogram.h>
-
-#include <boost/bind.hpp>
-#include <Eigen/Dense>
-#include <unsupported/Eigen/MatrixFunctions>
+// std header
 #include <iostream>
 #include <fstream>
 #include <numeric>

+// ROS headers
+#include <ros/ros.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/LaserScan.h>

+// Eigen headers
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <unsupported/Eigen/MatrixFunctions>
+
+namespace rf2o {
+
+template <typename T>
+inline T sign(const T x) { return x<T(0) ? -1:1; }
+
+template <typename Derived>
+inline typename Eigen::MatrixBase<Derived>::Scalar
+getYaw(const Eigen::MatrixBase<Derived>& r)
+{
+  return std::atan2( r(1, 0), r(0, 0) );
+}
+
+template<typename T>
+inline Eigen::Matrix<T, 3, 3> matrixRollPitchYaw(const T roll,
+                                                 const T pitch,
+                                                 const T yaw)
+{
+  const Eigen::AngleAxis<T> ax = Eigen::AngleAxis<T>(roll,  Eigen::Matrix<T, 3, 1>::UnitX());
+  const Eigen::AngleAxis<T> ay = Eigen::AngleAxis<T>(pitch, Eigen::Matrix<T, 3, 1>::UnitY());
+  const Eigen::AngleAxis<T> az = Eigen::AngleAxis<T>(yaw,   Eigen::Matrix<T, 3, 1>::UnitZ());
+
+  return (az * ay * ax).toRotationMatrix().matrix();
+}
+
+template<typename T>
+inline Eigen::Matrix<T, 3, 3> matrixYaw(const T yaw)
+{
+  return matrixRollPitchYaw<T>(0, 0, yaw);
+}

 class CLaserOdometry2D
 {
 public:
-	CLaserOdometry2D();
-    ~CLaserOdometry2D();

-    void Init(const sensor_msgs::LaserScan& scan,
-              const geometry_msgs::Pose& initial_robot_pose);
+  using Scalar = float;

-    bool is_initialized();
+  using Pose2d = Eigen::Isometry2d;
+  using Pose3d = Eigen::Isometry3d;
+  using MatrixS31 = Eigen::Matrix<Scalar, 3, 1>;
+  using IncrementCov = Eigen::Matrix<Scalar, 3, 3>;

-    void odometryCalculation(const sensor_msgs::LaserScan& scan);
+  CLaserOdometry2D();
+  virtual ~CLaserOdometry2D() = default;

-    void setLaserPose(const mrpt::poses::CPose3D& laser_pose);
+  void init(const sensor_msgs::LaserScan& scan,
+            const geometry_msgs::Pose& initial_robot_pose);

-    const mrpt::poses::CPose3D& getIncrement() const;
+  bool is_initialized();

-    const Eigen::Matrix<float, 3, 3>& getIncrementCovariance() const;
+  bool odometryCalculation(const sensor_msgs::LaserScan& scan);

-    mrpt::poses::CPose3D& getPose();
-    const mrpt::poses::CPose3D& getPose() const;
+  void setLaserPose(const Pose3d& laser_pose);
+
+  const Pose3d& getIncrement() const;
+
+  const IncrementCov& getIncrementCovariance() const;
+
+  Pose3d& getPose();
+  const Pose3d& getPose() const;

 protected:

-  bool verbose,module_initialized,first_laser_scan;
+  bool verbose, module_initialized, first_laser_scan;

-    // Internal Data
-	std::vector<Eigen::MatrixXf> range;
-	std::vector<Eigen::MatrixXf> range_old;
-	std::vector<Eigen::MatrixXf> range_inter;
-	std::vector<Eigen::MatrixXf> range_warped;
-	std::vector<Eigen::MatrixXf> xx;
-	std::vector<Eigen::MatrixXf> xx_inter;
-	std::vector<Eigen::MatrixXf> xx_old;
-	std::vector<Eigen::MatrixXf> xx_warped;
-	std::vector<Eigen::MatrixXf> yy;
-	std::vector<Eigen::MatrixXf> yy_inter;
-	std::vector<Eigen::MatrixXf> yy_old;
-	std::vector<Eigen::MatrixXf> yy_warped;
-	std::vector<Eigen::MatrixXf> transformations;
+  // Internal Data
+  std::vector<Eigen::MatrixXf> range;
+  std::vector<Eigen::MatrixXf> range_old;
+  std::vector<Eigen::MatrixXf> range_inter;
+  std::vector<Eigen::MatrixXf> range_warped;
+  std::vector<Eigen::MatrixXf> xx;
+  std::vector<Eigen::MatrixXf> xx_inter;
+  std::vector<Eigen::MatrixXf> xx_old;
+  std::vector<Eigen::MatrixXf> xx_warped;
+  std::vector<Eigen::MatrixXf> yy;
+  std::vector<Eigen::MatrixXf> yy_inter;
+  std::vector<Eigen::MatrixXf> yy_old;
+  std::vector<Eigen::MatrixXf> yy_warped;
+  std::vector<Eigen::MatrixXf> transformations;

-	Eigen::MatrixXf range_wf;
-	Eigen::MatrixXf dtita;
-	Eigen::MatrixXf dt;
-	Eigen::MatrixXf rtita;
-	Eigen::MatrixXf normx, normy, norm_ang;
-	Eigen::MatrixXf weights;
-	Eigen::MatrixXi null;
+  Eigen::MatrixXf range_wf;
+  Eigen::MatrixXf dtita;
+  Eigen::MatrixXf dt;
+  Eigen::MatrixXf rtita;
+  Eigen::MatrixXf normx, normy, norm_ang;
+  Eigen::MatrixXf weights;
+  Eigen::MatrixXi null;

-    Eigen::MatrixXf A,Aw;
-    Eigen::MatrixXf B,Bw;
-	Eigen::Matrix<float, 3, 1> Var;	//3 unknowns: vx, vy, w
-	Eigen::Matrix<float, 3, 3> cov_odo;
+  Eigen::MatrixXf A,Aw;
+  Eigen::MatrixXf B,Bw;

+  MatrixS31 Var;	//3 unknowns: vx, vy, w
+  IncrementCov cov_odo;

-
-    //std::string LaserVarName;				//Name of the topic containing the scan lasers \laser_scan
-	float fps;								//In Hz
-	float fovh;								//Horizontal FOV
-	unsigned int cols;
-	unsigned int cols_i;
-	unsigned int width;
-	unsigned int ctf_levels;
-	unsigned int image_level, level;
-	unsigned int num_valid_range;
-    unsigned int iter_irls;
-	float g_mask[5];
+  //std::string LaserVarName;				//Name of the topic containing the scan lasers \laser_scan
+  float fps;								//In Hz
+  float fovh;								//Horizontal FOV
+  unsigned int cols;
+  unsigned int cols_i;
+  unsigned int width;
+  unsigned int ctf_levels;
+  unsigned int image_level, level;
+  unsigned int num_valid_range;
+  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, current_scan_time;
+  ros::WallDuration	m_runtime;
+  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;
+  MatrixS31 kai_abs_;
+  MatrixS31 kai_loc_;
+  MatrixS31 kai_loc_old_;
+  MatrixS31 kai_loc_level_;

-  mrpt::poses::CPose3D last_increment;
+  Pose3d last_increment_;
+  Pose3d laser_pose_on_robot_;
+  Pose3d laser_pose_on_robot_inv_;
+  Pose3d laser_pose_;
+  Pose3d laser_oldpose_;
+  Pose3d robot_pose_;
+  Pose3d robot_oldpose_;

-  mrpt::poses::CPose3D laser_pose_on_robot;
-  mrpt::poses::CPose3D laser_pose_on_robot_inv;
+  bool test;
+  std::vector<double> last_m_lin_speeds;
+  std::vector<double> last_m_ang_speeds;

-  mrpt::poses::CPose3D laser_pose;
-	mrpt::poses::CPose3D laser_oldpose;
-    mrpt::poses::CPose3D robot_pose;
-    mrpt::poses::CPose3D robot_oldpose;
-	bool test;
-    std::vector<double> last_m_lin_speeds;
-    std::vector<double> last_m_ang_speeds;
-
-
-	// Methods
-	void createImagePyramid();
-	void calculateCoord();
-	void performWarping();
-	void calculaterangeDerivativesSurface();
-	void computeNormals();
-	void computeWeights();
-	void findNullPoints();
-	void solveSystemOneLevel();
-    void solveSystemNonLinear();
-	void filterLevelSolution();
-	void PoseUpdate();
-    void Reset(mrpt::poses::CPose3D ini_pose, CObservation2DRangeScan scan);
+  // Methods
+  void createImagePyramid();
+  void calculateCoord();
+  void performWarping();
+  void calculaterangeDerivativesSurface();
+  void computeNormals();
+  void computeWeights();
+  void findNullPoints();
+  void solveSystemOneLevel();
+  void solveSystemNonLinear();
+  bool filterLevelSolution();
+  void PoseUpdate();
+  void Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan scan*/);
 };

-#endif
+} /* namespace rf2o */
+
+#endif // CLaserOdometry2D_H
--- a/package.xml
+++ b/package.xml
@@ -24,15 +24,14 @@
  <build_depend>tf</build_depend>
  <build_depend>cmake_modules</build_depend>   <!-- A common repository for CMake Modules which are not distributed with CMake but are commonly used by ROS packages. -->
                                               <!-- https://github.com/ros/cmake_modules/blob/0.3-devel/README.md -->
-  <build_depend>mrpt</build_depend>            <!-- Depend on mrpt system pkgs: http://www.mrpt.org/ -->
-
+  <build_depend>eigen</build_depend>

  <run_depend>nav_msgs</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>sensor_msgs</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>tf</run_depend>
-  <run_depend>cmake_modules</run_depend>       <!-- For aditional dependencies such as Eigen -->
-  <run_depend>mrpt</run_depend>                <!-- Depend on mrpt system pkgs -->
+  <run_depend>cmake_modules</run_depend>   <!-- For aditional dependencies such as Eigen -->
+  <run_depend>eigen</run_depend>

 </package>
--- a/src/CLaserOdometry2D.cpp
+++ b/src/CLaserOdometry2D.cpp
--- a/src/CLaserOdometry2DNode.cpp
+++ b/src/CLaserOdometry2DNode.cpp
@@ -11,6 +11,8 @@
 *
 * Maintainer: Javier G. Monroy
 * MAPIR group: http://mapir.isa.uma.es/
+*
+* Modifications: Jeremie Deray
 ******************************************************************************************** */

 #include "rf2o_laser_odometry/CLaserOdometry2D.h"
@@ -18,12 +20,14 @@
 #include <tf/transform_broadcaster.h>
 #include <tf/transform_listener.h>

+namespace rf2o {
+
 class CLaserOdometry2DNode : CLaserOdometry2D
 {
 public:

  CLaserOdometry2DNode();
-  ~CLaserOdometry2DNode();
+  ~CLaserOdometry2DNode() = default;

  void process(const ros::TimerEvent &);
  void publish();
@@ -32,7 +36,7 @@ public:

 public:

-  bool publish_tf,new_scan_available;
+  bool publish_tf, new_scan_available;

  double freq;

@@ -63,57 +67,52 @@ public:
 CLaserOdometry2DNode::CLaserOdometry2DNode() :
  CLaserOdometry2D()
 {
-    ROS_INFO("Initializing RF2O node...");
+  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);
+  //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);
+  //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;
-    }
+  //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();
+  setLaserPoseFromTf();

-    //Init variables
-    module_initialized = false;
-    first_laser_scan   = true;
+  //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;
@@ -124,28 +123,31 @@ bool CLaserOdometry2DNode::setLaserPoseFromTf()
  transform.setIdentity();
  try
  {
-      tf_listener.lookupTransform(base_frame_id, last_scan.header.frame_id, ros::Time(0), transform);
-      retrieved = true;
+    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;
+    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];
+  //TF:transform -> Eigen::Isometry3d
+
  const tf::Matrix3x3 &basis = transform.getBasis();
-  mrpt::math::CMatrixDouble33 R;
+  Eigen::Matrix3d 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);
+    for(int c = 0; c < 3; c++)
+      R(r,c) = basis[r][c];
+
+  Pose3d laser_tf(R);
+
+  const tf::Vector3 &t = transform.getOrigin();
+  laser_tf.translation()(0) = t[0];
+  laser_tf.translation()(1) = t[1];
+  laser_tf.translation()(2) = t[2];

  setLaserPose(laser_tf);

@@ -154,21 +156,21 @@ bool CLaserOdometry2DNode::setLaserPoseFromTf()

 bool CLaserOdometry2DNode::scan_available()
 {
-    return new_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
+    //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....") ;
+    ROS_WARN("Waiting for laser_scans....") ;
  }
 }

@@ -178,36 +180,36 @@ void CLaserOdometry2DNode::process(const ros::TimerEvent&)

 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;
+  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;
-        }
+    //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;
-    }
+  //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()
@@ -216,17 +218,17 @@ void CLaserOdometry2DNode::publish()
  //---------------------------------------
  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);
+    //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_.translation()(0);
+    odom_trans.transform.translation.y = robot_pose_.translation()(1);
+    odom_trans.transform.translation.z = 0.0;
+    odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(rf2o::getYaw(robot_pose_.rotation()));
+    //send the transform
+    odom_broadcaster.sendTransform(odom_trans);
  }

  //next, we'll publish the odometry message over ROS
@@ -236,10 +238,10 @@ void CLaserOdometry2DNode::publish()
  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.x = robot_pose_.translation()(0);
+  odom.pose.pose.position.y = robot_pose_.translation()(1);
  odom.pose.pose.position.z = 0.0;
-  odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(robot_pose.yaw());
+  odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(rf2o::getYaw(robot_pose_.rotation()));
  //set the velocity
  odom.child_frame_id = base_frame_id;
  odom.twist.twist.linear.x = lin_speed;    //linear speed
@@ -249,27 +251,29 @@ void CLaserOdometry2DNode::publish()
  odom_pub.publish(odom);
 }

+} /* namespace rf2o */
+
 //-----------------------------------------------------------------------------------
 //                                   MAIN
 //-----------------------------------------------------------------------------------
 int main(int argc, char** argv)
 {
-    ros::init(argc, argv, "RF2O_LaserOdom");
+  ros::init(argc, argv, "RF2O_LaserOdom");

-    CLaserOdometry2DNode myLaserOdomNode;
+  rf2o::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();
+  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();
+  timer_opt.callback = boost::bind(&rf2o::CLaserOdometry2DNode::process, &myLaserOdomNode, _1);
+  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;
 }