copy from old "mapir_ros_sources"

CMakeLists.txt

@@ -0,0 +1,63 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rf2o_laser_odometry)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS  
+  roscpp
+  rospy
+  nav_msgs
+  sensor_msgs
+  std_msgs
+  tf  
+)
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED COMPONENTS system)
+find_package(cmake_modules REQUIRED)
+find_package(Eigen 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_LIBS})
+
+
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES laser_odometry
+ CATKIN_DEPENDS nav_msgs roscpp sensor_msgs std_msgs tf
+ DEPENDS system_lib
+)
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(include)
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  ${EIGEN_INCLUDE_DIRS}
+)
+
+## Declare a cpp executable
+add_executable(rf2o_laser_odometry_node 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}
+)

include/rf2o_laser_odometry/CLaserOdometry2D.h

@@ -0,0 +1,159 @@
+/** ****************************************************************************************
+*  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/
+******************************************************************************************** */
+
+#ifndef CLaserOdometry2D_H
+#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
+// MRPT related headers
+#include <mrpt/version.h>
+#if MRPT_VERSION>=0x130
+#	include <mrpt/obs/CObservation2DRangeScan.h>
+#   include <mrpt/obs/CObservationOdometry.h>
+    using namespace mrpt::obs;
+#else
+#	include <mrpt/slam/CObservation2DRangeScan.h>
+#   include <mrpt/slam/CObservationOdometry.h>
+    using namespace mrpt::slam;
+#endif
+#include <mrpt/system/threads.h>
+#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>
+#include <iostream>
+#include <fstream>
+#include <numeric>
+
+
+
+class CLaserOdometry2D
+{
+public:
+	CLaserOdometry2D();
+    ~CLaserOdometry2D();
+    bool is_initialized();
+    bool scan_available();
+    void Init();
+    void odometryCalculation();
+
+    std::string laser_scan_topic;
+    std::string base_frame_id;
+    std::string odom_frame_id;
+    double freq;
+
+protected:
+    ros::NodeHandle n;
+    sensor_msgs::LaserScan last_scan;    
+    bool module_initialized,first_laser_scan,new_scan_available;
+    tf::TransformListener tf_listener;          //Do not put inside the callback
+    tf::TransformBroadcaster odom_broadcaster;
+
+    //Subscriptions & Publishers
+    ros::Subscriber laser_sub;
+    ros::Publisher odom_pub;
+
+    //CallBacks
+    void LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_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;
+	
+	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;
+
+
+
+    //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];
+
+    //mrpt::gui::CDisplayWindowPlots window;
+	mrpt::utils::CTicTac		m_clock;
+	float		m_runtime;
+    ros::Time last_odom_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 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);
+};
+
+#endif

launch/rf2o_laser_odometry.launch

@@ -0,0 +1,19 @@
+<!-- 
+  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.
+  
+  Requirements:
+  - 2D laser scan, publishing sensor_msgs::LaserScan
+  - TF transform from the laser to the robot base
+-->
+
+<launch>
+
+  <node pkg="rf2o_laser_odometry" type="rf2o_laser_odometry_node" name="rf2o_laser_odometry">
+    <param name="laser_scan_topic" value="/laser_scan"/>        <!-- topic where the lidar scans are being published -->
+    <param name="base_frame_id" value="/base_link"/>            <!-- frame_id (tf) of the mobile robot base. A tf transform from the laser_frame to the base_frame is mandatory -->
+    <param name="odom_frame_id" value="/odom" />                <!-- frame_id (tf) to publish the odometry estimations -->
+    <param name="freq" value="6.0"/>                            <!-- Execution frequency. See "Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA'16"-->
+  </node>
+</launch>

package.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<package>
+
+  <name>rf2o_laser_odometry</name>
+  <version>1.0.0</version>
+  <description>
+    Estimation of 2D odometry based on planar laser scans. Useful for mobile robots with innacurate base 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/mapirwebsite/index.php/mapir-downloads/papers/217
+  </description>
+
+  <maintainer email="jgmonroy@uma.es">Javier G. Monroy</maintainer>
+  <author email="marianojt@uma.es"> Mariano Jaimez</author>
+  <author email="jgmonroy@uma.es"> Javier G. Monroy</author>
+  <license>GPL v3</license> 
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <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/ -->
+
+
+  <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 -->
+
+</package>