diff --git a/CMakeLists.txt b/CMakeLists.txt index 168957f..ac877a2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,23 +18,13 @@ find_package(catkin REQUIRED COMPONENTS nav_msgs sensor_msgs std_msgs - tf + 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) diff --git a/README.md b/README.md index 0f7b632..bb8d153 100644 --- a/README.md +++ b/README.md @@ -5,9 +5,4 @@ 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 +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 \ No newline at end of file diff --git a/include/rf2o_laser_odometry/CLaserOdometry2D.h b/include/rf2o_laser_odometry/CLaserOdometry2D.h index caef3b0..d46a60d 100644 --- 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 -#include -#include - -// MRPT related headers -#include -#if MRPT_VERSION>=0x130 -# include -# include - typedef mrpt::obs::CObservation2DRangeScan CObservation2DRangeScan; -#else -# include -# include - typedef mrpt::poses::CObservation2DRangeScan CObservation2DRangeScan; -#endif - -#if MRPT_VERSION<0x150 -# include -#endif - -#include -#include -#include -//#include -#include - -#include -#include -#include +// std header #include #include #include +// ROS headers +#include +#include +#include +// Eigen headers +#include +#include +#include + +namespace rf2o { + +template +inline T sign(const T x) { return x +inline typename Eigen::MatrixBase::Scalar +getYaw(const Eigen::MatrixBase& r) +{ + return std::atan2( r(1, 0), r(0, 0) ); +} + +template +inline Eigen::Matrix matrixRollPitchYaw(const T roll, + const T pitch, + const T yaw) +{ + const Eigen::AngleAxis ax = Eigen::AngleAxis(roll, Eigen::Matrix::UnitX()); + const Eigen::AngleAxis ay = Eigen::AngleAxis(pitch, Eigen::Matrix::UnitY()); + const Eigen::AngleAxis az = Eigen::AngleAxis(yaw, Eigen::Matrix::UnitZ()); + + return (az * ay * ax).toRotationMatrix().matrix(); +} + +template +inline Eigen::Matrix matrixYaw(const T yaw) +{ + return matrixRollPitchYaw(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; + using IncrementCov = Eigen::Matrix; - 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& 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 range; - std::vector range_old; - std::vector range_inter; - std::vector range_warped; - std::vector xx; - std::vector xx_inter; - std::vector xx_old; - std::vector xx_warped; - std::vector yy; - std::vector yy_inter; - std::vector yy_old; - std::vector yy_warped; - std::vector transformations; + // Internal Data + std::vector range; + std::vector range_old; + std::vector range_inter; + std::vector range_warped; + std::vector xx; + std::vector xx_inter; + std::vector xx_old; + std::vector xx_warped; + std::vector yy; + std::vector yy_inter; + std::vector yy_old; + std::vector yy_warped; + std::vector 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 Var; //3 unknowns: vx, vy, w - Eigen::Matrix 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 last_m_lin_speeds; + std::vector 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 last_m_lin_speeds; - std::vector 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 diff --git a/package.xml b/package.xml index dc80124..d39fb72 100644 --- a/package.xml +++ b/package.xml @@ -24,15 +24,14 @@ tf cmake_modules - mrpt - + eigen nav_msgs roscpp sensor_msgs std_msgs tf - cmake_modules - mrpt + cmake_modules + eigen diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp index 12c4c5b..6894730 100644 --- a/src/CLaserOdometry2D.cpp +++ b/src/CLaserOdometry2D.cpp @@ -11,156 +11,157 @@ * * Maintainer: Javier G. Monroy * MAPIR group: http://mapir.isa.uma.es/ +* +* Modifications: Jeremie Deray ******************************************************************************************** */ #include "rf2o_laser_odometry/CLaserOdometry2D.h" +namespace rf2o { + // -------------------------------------------- // CLaserOdometry2D //--------------------------------------------- CLaserOdometry2D::CLaserOdometry2D() : + verbose(false), module_initialized(false), - first_laser_scan(true) + first_laser_scan(true), + last_increment_(Pose3d::Identity()), + laser_pose_on_robot_(Pose3d::Identity()), + laser_pose_on_robot_inv_(Pose3d::Identity()), + laser_pose_(Pose3d::Identity()), + laser_oldpose_(Pose3d::Identity()), + robot_pose_(Pose3d::Identity()), + robot_oldpose_(Pose3d::Identity()) { // } - -CLaserOdometry2D::~CLaserOdometry2D() -{ - // -} - -void CLaserOdometry2D::setLaserPose(const mrpt::poses::CPose3D& laser_pose) +void CLaserOdometry2D::setLaserPose(const Pose3d& 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(); + laser_pose_on_robot_ = laser_pose; + laser_pose_on_robot_inv_ = laser_pose_on_robot_.inverse(); } bool CLaserOdometry2D::is_initialized() { - return module_initialized; + return module_initialized; } -void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan, +void CLaserOdometry2D::init(const sensor_msgs::LaserScan& scan, const geometry_msgs::Pose& initial_robot_pose) { - //Got an initial scan laser, obtain its parametes - ROS_INFO_COND(verbose, "[rf2o] Got first Laser Scan .... Configuring node"); + //Got an initial scan laser, obtain its parametes + ROS_INFO_COND(verbose, "[rf2o] Got first Laser Scan .... Configuring node"); - width = 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(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 + cols = width; // Max resolution. Should be similar to the width parameter + fovh = std::abs(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 - //Robot initial pose (see MQTT:bridge) - mrpt::poses::CPose3D robotInitialPose; - geometry_msgs::Pose _src = initial_robot_pose; + Pose3d robot_initial_pose = Pose3d::Identity(); - robotInitialPose.x(_src.position.x); - robotInitialPose.y(_src.position.y); + robot_initial_pose = Eigen::Quaterniond(initial_robot_pose.orientation.w, + initial_robot_pose.orientation.x, + initial_robot_pose.orientation.y, + initial_robot_pose.orientation.z); - mrpt::math::CQuaternionDouble quat; - quat.x(_src.orientation.x); - quat.y(_src.orientation.y); - quat.z(_src.orientation.z); - quat.r(_src.orientation.w); - double roll, pitch, yaw; - quat.rpy(roll, pitch, yaw); - robotInitialPose.setYawPitchRoll(yaw,pitch,roll); - //robotInitialPose.phi(yaw); + robot_initial_pose.translation()(0) = initial_robot_pose.position.x; + robot_initial_pose.translation()(1) = initial_robot_pose.position.y; - //Set the initial pose - laser_pose = robotInitialPose + laser_pose_on_robot; - laser_oldpose = robotInitialPose + laser_pose_on_robot; + ROS_INFO_STREAM_COND(verbose, "[rf2o] Setting origin at:\n" + << robot_initial_pose.matrix()); - // Init module (internal) - //------------------------ - range_wf.setSize(1, width); + //Set the initial pose + laser_pose_ = robot_initial_pose * laser_pose_on_robot_; + laser_oldpose_ = laser_oldpose_; - //Resize vectors according to levels - transformations.resize(ctf_levels); - for (unsigned int i = 0; i < ctf_levels; i++) - transformations[i].resize(3, 3); + // Init module (internal) + //------------------------ + range_wf = Eigen::MatrixXf::Constant(1, width, 1); - //Resize pyramid - unsigned int s, cols_i; - const unsigned int pyr_levels = round(log2(round(float(width) / float(cols)))) + ctf_levels; - range.resize(pyr_levels); - range_old.resize(pyr_levels); - range_inter.resize(pyr_levels); - xx.resize(pyr_levels); - xx_inter.resize(pyr_levels); - xx_old.resize(pyr_levels); - yy.resize(pyr_levels); - yy_inter.resize(pyr_levels); - yy_old.resize(pyr_levels); - range_warped.resize(pyr_levels); - xx_warped.resize(pyr_levels); - yy_warped.resize(pyr_levels); + //Resize vectors according to levels + transformations.resize(ctf_levels); + for (unsigned int i = 0; i < ctf_levels; i++) + transformations[i].resize(3, 3); - for (unsigned int i = 0; i < pyr_levels; i++) + //Resize pyramid + unsigned int s, cols_i; + const unsigned int pyr_levels = std::round(std::log2(round(float(width) / float(cols)))) + ctf_levels; + range.resize(pyr_levels); + range_old.resize(pyr_levels); + range_inter.resize(pyr_levels); + xx.resize(pyr_levels); + xx_inter.resize(pyr_levels); + xx_old.resize(pyr_levels); + yy.resize(pyr_levels); + yy_inter.resize(pyr_levels); + yy_old.resize(pyr_levels); + range_warped.resize(pyr_levels); + xx_warped.resize(pyr_levels); + yy_warped.resize(pyr_levels); + + for (unsigned int i = 0; i < pyr_levels; i++) + { + s = std::pow(2.f, int(i)); + cols_i = std::ceil(float(width) / float(s)); + + range[i] = Eigen::MatrixXf::Constant(1, cols_i, 0.f); + range_old[i] = Eigen::MatrixXf::Constant(1, cols_i, 0.f); + range_inter[i].resize(1, cols_i); + + xx[i] = Eigen::MatrixXf::Constant(1, cols_i, 0.f); + xx_old[i] = Eigen::MatrixXf::Constant(1, cols_i, 0.f); + + yy[i] = Eigen::MatrixXf::Constant(1, cols_i, 0.f); + yy_old[i] = Eigen::MatrixXf::Constant(1, cols_i, 0.f); + + xx_inter[i].resize(1, cols_i); + yy_inter[i].resize(1, cols_i); + + if (cols_i <= cols) { - s = pow(2.f, int(i)); - cols_i = ceil(float(width) / float(s)); - - range[i].resize(1, cols_i); - range_inter[i].resize(1, cols_i); - range_old[i].resize(1, cols_i); - range[i].assign(0.0f); - range_old[i].assign(0.0f); - xx[i].resize(1, cols_i); - xx_inter[i].resize(1, cols_i); - xx_old[i].resize(1, cols_i); - xx[i].assign(0.0f); - xx_old[i].assign(0.0f); - yy[i].resize(1, cols_i); - yy_inter[i].resize(1, cols_i); - yy_old[i].resize(1, cols_i); - yy[i].assign(0.f); - yy_old[i].assign(0.f); - - if (cols_i <= cols) - { - range_warped[i].resize(1, cols_i); - xx_warped[i].resize(1, cols_i); - yy_warped[i].resize(1, cols_i); - } + range_warped[i].resize(1, cols_i); + xx_warped[i].resize(1, cols_i); + yy_warped[i].resize(1, cols_i); } + } - dt.resize(1, cols); - dtita.resize(1, cols); - normx.resize(1, cols); - normy.resize(1, cols); - norm_ang.resize(1, cols); - weights.setSize(1, cols); - null.setSize(1, cols); - null.assign(0); - cov_odo.assign(0.f); + dt.resize(1, cols); + dtita.resize(1, cols); + normx.resize(1, cols); + normy.resize(1, cols); + norm_ang.resize(1, cols); + weights.resize(1, cols); + null = Eigen::MatrixXi::Constant(1, cols, 0); + cov_odo = IncrementCov::Zero(); - fps = 1.f; //In Hz - num_valid_range = 0; + fps = 1.f; //In Hz + num_valid_range = 0; - //Compute gaussian mask - g_mask[0] = 1.f / 16.f; g_mask[1] = 0.25f; g_mask[2] = 6.f / 16.f; g_mask[3] = g_mask[1]; g_mask[4] = g_mask[0]; + //Compute gaussian mask + g_mask[0] = 1.f / 16.f; + g_mask[1] = 0.25f; + g_mask[2] = 6.f / 16.f; + g_mask[3] = g_mask[1]; + g_mask[4] = g_mask[0]; - kai_abs.assign(0.f); - kai_loc_old.assign(0.f); + kai_abs_ = MatrixS31::Zero(); + kai_loc_old_ = MatrixS31::Zero(); - module_initialized = true; - last_odom_time = ros::Time::now(); + module_initialized = true; + last_odom_time = ros::Time::now(); } -const mrpt::poses::CPose3D& CLaserOdometry2D::getIncrement() const +const CLaserOdometry2D::Pose3d& CLaserOdometry2D::getIncrement() const { - return last_increment; + return last_increment_; } const Eigen::Matrix& CLaserOdometry2D::getIncrementCovariance() const @@ -168,784 +169,806 @@ const Eigen::Matrix& CLaserOdometry2D::getIncrementCovariance() con return cov_odo; } -mrpt::poses::CPose3D& CLaserOdometry2D::getPose() +CLaserOdometry2D::Pose3d& CLaserOdometry2D::getPose() { - return robot_pose; + return robot_pose_; } -const mrpt::poses::CPose3D& CLaserOdometry2D::getPose() const +const CLaserOdometry2D::Pose3d& CLaserOdometry2D::getPose() const { - return robot_pose; + return robot_pose_; } -void CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan) +bool CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan) { - //================================================================================== - // DIFERENTIAL ODOMETRY MULTILEVEL - //================================================================================== + //================================================================================== + // DIFERENTIAL ODOMETRY MULTILEVEL + //================================================================================== - //copy laser scan to internal variable - for (unsigned int i = 0; i(scan.ranges.data(), width, 1); - m_clock.Tic(); - createImagePyramid(); + ros::WallTime start = ros::WallTime::now(); - //Coarse-to-fine scheme - for (unsigned int i=0; i 3) - { - solveSystemNonLinear(); - //solveSystemOneLevel(); //without robust-function - } - - //8. Filter solution - filterLevelSolution(); + //7. Solve odometry + if (num_valid_range > 3) + { + solveSystemNonLinear(); + //solveSystemOneLevel(); //without robust-function + } + else + { + /// @todo At initialization something + /// isn't properly initialized so that + /// uninitialized values get propagated + /// from 'filterLevelSolution' first call + /// Throughout the whole execution. Thus + /// this 'continue' that surprisingly works. + continue; } - m_runtime = 1000*m_clock.Tac(); + //8. Filter solution + if (!filterLevelSolution()) return false; + } - ROS_INFO_COND(verbose, "[rf2o] execution time (ms): %f", m_runtime); + m_runtime = ros::WallTime::now() - start; - //Update poses - PoseUpdate(); + ROS_INFO_COND(verbose, "[rf2o] execution time (ms): %f", + m_runtime.toSec()*double(1000)); + + //Update poses + PoseUpdate(); + + return true; } - void CLaserOdometry2D::createImagePyramid() { - const float max_range_dif = 0.3f; + const float max_range_dif = 0.3f; - //Push the frames back - range_old.swap(range); - xx_old.swap(xx); - yy_old.swap(yy); + //Push the frames back + range_old.swap(range); + xx_old.swap(xx); + yy_old.swap(yy); - //The number of levels of the pyramid does not match the number of levels used - //in the odometry computation (because we sometimes want to finish with lower resolutions) + //The number of levels of the pyramid does not match the number of levels used + //in the odometry computation (because we sometimes want to finish with lower resolutions) - unsigned int pyr_levels = round(log2(round(float(width)/float(cols)))) + ctf_levels; + unsigned int pyr_levels = std::round(std::log2(std::round(float(width)/float(cols)))) + ctf_levels; - //Generate levels - for (unsigned int i = 0; i Filter (not downsampling); + if (i == 0) { - unsigned int s = pow(2.f,int(i)); - cols_i = ceil(float(width)/float(s)); + for (unsigned int u = 0; u < cols_i; u++) + { + const float dcenter = range_wf(u); - const unsigned int i_1 = i-1; - - //First level -> Filter (not downsampling); - if (i == 0) - { - for (unsigned int u = 0; u < cols_i; u++) - { - const float dcenter = range_wf(u); - - //Inner pixels - if ((u>1)&&(u 0.f) - { - float sum = 0.f; - float weight = 0.f; - - for (int l=-2; l<3; l++) - { - const float abs_dif = abs(range_wf(u+l)-dcenter); - if (abs_dif < max_range_dif) - { - const float aux_w = g_mask[2+l]*(max_range_dif - abs_dif); - weight += aux_w; - sum += aux_w*range_wf(u+l); - } - } - range[i](u) = sum/weight; - } - else - range[i](u) = 0.f; - - } - - //Boundary - else - { - if (dcenter > 0.f) - { - float sum = 0.f; - float weight = 0.f; - - for (int l=-2; l<3; l++) - { - const int indu = u+l; - if ((indu>=0)&&(indu1)&&(u 0.f) + { + float sum = 0.f; + float weight = 0.f; + + for (int l=-2; l<3; l++) { - const int u2 = 2*u; - const float dcenter = range[i_1](u2); - - //Inner pixels - if ((u>0)&&(u 0.f) - { - float sum = 0.f; - float weight = 0.f; - - for (int l=-2; l<3; l++) - { - const float abs_dif = abs(range[i_1](u2+l)-dcenter); - if (abs_dif < max_range_dif) - { - const float aux_w = g_mask[2+l]*(max_range_dif - abs_dif); - weight += aux_w; - sum += aux_w*range[i_1](u2+l); - } - } - range[i](u) = sum/weight; - } - else - range[i](u) = 0.f; - - } - - //Boundary - else - { - if (dcenter > 0.f) - { - float sum = 0.f; - float weight = 0.f; - const unsigned int cols_i2 = range[i_1].cols(); - - - for (int l=-2; l<3; l++) - { - const int indu = u2+l; - if ((indu>=0)&&(indu 0.f) - { - const float tita = -0.5*fovh + float(u)*fovh/float(cols_i-1); - xx[i](u) = range[i](u)*cos(tita); - yy[i](u) = range[i](u)*sin(tita); - } - else - { - xx[i](u) = 0.f; - yy[i](u) = 0.f; - } - } + //Boundary + else + { + if (dcenter > 0.f) + { + float sum = 0.f; + float weight = 0.f; + + for (int l=-2; l<3; l++) + { + const int indu = u+l; + if ((indu>=0)&&(indu0)&&(u 0.f) + { + float sum = 0.f; + float weight = 0.f; + + for (int l=-2; l<3; l++) + { + const float abs_dif = std::abs(range[i_1](u2+l)-dcenter); + if (abs_dif < max_range_dif) + { + const float aux_w = g_mask[2+l]*(max_range_dif - abs_dif); + weight += aux_w; + sum += aux_w*range[i_1](u2+l); + } + } + range[i](u) = sum/weight; + } + else + range[i](u) = 0.f; + + } + + //Boundary + else + { + if (dcenter > 0.f) + { + float sum = 0.f; + float weight = 0.f; + const unsigned int cols_i2 = range[i_1].cols(); + + + for (int l=-2; l<3; l++) + { + const int indu = u2+l; + if ((indu>=0)&&(indu 0.f) + { + const float tita = -0.5*fovh + float(u)*fovh/float(cols_i-1); + xx[i](u) = range[i](u)*std::cos(tita); + yy[i](u) = range[i](u)*std::sin(tita); + } + else + { + xx[i](u) = 0.f; + yy[i](u) = 0.f; + } + } + } } - - void CLaserOdometry2D::calculateCoord() { - for (unsigned int u = 0; u < cols_i; u++) - { - if ((range_old[image_level](u) == 0.f) || (range_warped[image_level](u) == 0.f)) - { - range_inter[image_level](u) = 0.f; - xx_inter[image_level](u) = 0.f; - yy_inter[image_level](u) = 0.f; - } - else - { - range_inter[image_level](u) = 0.5f*(range_old[image_level](u) + range_warped[image_level](u)); - xx_inter[image_level](u) = 0.5f*(xx_old[image_level](u) + xx_warped[image_level](u)); - yy_inter[image_level](u) = 0.5f*(yy_old[image_level](u) + yy_warped[image_level](u)); - } - } + for (unsigned int u = 0; u < cols_i; u++) + { + if ((range_old[image_level](u) == 0.f) || (range_warped[image_level](u) == 0.f)) + { + range_inter[image_level](u) = 0.f; + xx_inter[image_level](u) = 0.f; + yy_inter[image_level](u) = 0.f; + } + else + { + range_inter[image_level](u) = 0.5f*(range_old[image_level](u) + range_warped[image_level](u)); + xx_inter[image_level](u) = 0.5f*(xx_old[image_level](u) + xx_warped[image_level](u)); + yy_inter[image_level](u) = 0.5f*(yy_old[image_level](u) + yy_warped[image_level](u)); + } + } } - void CLaserOdometry2D::calculaterangeDerivativesSurface() { - //The gradient size ir reserved at the maximum size (at the constructor) + //The gradient size ir reserved at the maximum size (at the constructor) - //Compute connectivity - rtita.resize(1,cols_i); //Defined in a different way now, without inversion - rtita.assign(1.f); + //Compute connectivity - for (unsigned int u = 0; u < cols_i-1; u++) - { - const float dist = mrpt::math::square(xx_inter[image_level](u+1) - xx_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); - } + //Defined in a different way now, without inversion + rtita = Eigen::MatrixXf::Constant(1, cols_i, 1.f); - //Spatial derivatives - for (unsigned int u = 1; u < cols_i-1; u++) - dtita(u) = (rtita(u-1)*(range_inter[image_level](u+1)-range_inter[image_level](u)) + rtita(u)*(range_inter[image_level](u) - range_inter[image_level](u-1)))/(rtita(u)+rtita(u-1)); + for (unsigned int u = 0; u < cols_i-1; u++) + { + float dista = xx_inter[image_level](u+1) - xx_inter[image_level](u); + dista *= dista; + float distb = yy_inter[image_level](u+1) - yy_inter[image_level](u); + distb *= distb; + const float dist = dista + distb; - dtita(0) = dtita(1); - dtita(cols_i-1) = dtita(cols_i-2); + if (dist > 0.f) + rtita(u) = std::sqrt(dist); + } - //Temporal derivative - for (unsigned int u = 0; u < cols_i; u++) - dt(u) = fps*(range_warped[image_level](u) - range_old[image_level](u)); + //Spatial derivatives + for (unsigned int u = 1; u < cols_i-1; u++) + dtita(u) = (rtita(u-1)*(range_inter[image_level](u+1)- + range_inter[image_level](u)) + rtita(u)*(range_inter[image_level](u) - + range_inter[image_level](u-1)))/(rtita(u)+rtita(u-1)); + + dtita(0) = dtita(1); + dtita(cols_i-1) = dtita(cols_i-2); + + //Temporal derivative + for (unsigned int u = 0; u < cols_i; u++) + dt(u) = fps*(range_warped[image_level](u) - range_old[image_level](u)); - //Apply median filter to the range derivatives - //MatrixXf dtitamed = dtita, dtmed = dt; - //vector svector(3); - //for (unsigned int u=1; u svector(3); + //for (unsigned int u=1; u M_PI) - norm_ang(u) -= M_PI; + const float incr_tita = fovh/float(cols_i-1); + for (unsigned int u=0; u M_PI) + norm_ang(u) -= M_PI; - normx(u) = cos(tita + alfa); - normy(u) = sin(tita + alfa); - } - } + normx(u) = std::cos(tita + alfa); + normy(u) = std::sin(tita + alfa); + } + } } - void CLaserOdometry2D::computeWeights() { - //The maximum weight size is reserved at the constructor - weights.assign(0.f); + //The maximum weight size is reserved at the constructor + weights.setConstant(1, cols, 0.f); - //Parameters for error_linearization - const float kdtita = 1.f; - const float kdt = kdtita/mrpt::math::square(fps); - const float k2d = 0.2f; + //Parameters for error_linearization + const float kdtita = 1.f; + const float kdt = kdtita / (fps*fps); + const float k2d = 0.2f; - for (unsigned int u = 1; u < cols_i-1; u++) - if (null(u) == 0) - { - // Compute derivatives - //----------------------------------------------------------------------- - const float ini_dtita = range_old[image_level](u+1) - range_old[image_level](u-1); - const float final_dtita = range_warped[image_level](u+1) - range_warped[image_level](u-1); + for (unsigned int u = 1; u < cols_i-1; u++) + if (null(u) == 0) + { + // Compute derivatives + //----------------------------------------------------------------------- + const float ini_dtita = range_old[image_level](u+1) - range_old[image_level](u-1); + const float final_dtita = range_warped[image_level](u+1) - range_warped[image_level](u-1); - const float dtitat = ini_dtita - final_dtita; - const float dtita2 = dtita(u+1) - dtita(u-1); + const float dtitat = ini_dtita - final_dtita; + const float dtita2 = dtita(u+1) - dtita(u-1); - const float w_der = kdt*mrpt::math::square(dt(u)) + kdtita*mrpt::math::square(dtita(u)) + k2d*(abs(dtitat) + abs(dtita2)); + const float w_der = kdt*(dt(u)*dt(u)) + + kdtita*(dtita(u)*dtita(u)) + + k2d*(std::abs(dtitat) + std::abs(dtita2)); - weights(u) = sqrt(1.f/w_der); - } + weights(u) = std::sqrt(1.f/w_der); + } - const float inv_max = 1.f/weights.maximum(); - weights = inv_max*weights; + const float inv_max = 1.f / weights.maxCoeff(); + weights = inv_max*weights; } - void CLaserOdometry2D::findNullPoints() { - //Size of null matrix is set to its maximum size (constructor) - num_valid_range = 0; + //Size of null matrix is set to its maximum size (constructor) + num_valid_range = 0; - for (unsigned int u = 1; u < cols_i-1; u++) - { - if (range_inter[image_level](u) == 0.f) - null(u) = 1; - else - { - num_valid_range++; - null(u) = 0; - } - } + for (unsigned int u = 1; u < cols_i-1; u++) + { + if (range_inter[image_level](u) == 0.f) + null(u) = 1; + else + { + num_valid_range++; + null(u) = 0; + } + } } - // Solves the system without considering any robust-function void CLaserOdometry2D::solveSystemOneLevel() { - A.resize(num_valid_range,3); - B.setSize(num_valid_range,1); - unsigned int cont = 0; - const float kdtita = (cols_i-1)/fovh; + A.resize(num_valid_range, 3); + B.resize(num_valid_range, 1); - //Fill the matrix A and the vector B - //The order of the variables will be (vx, vy, wz) + unsigned int cont = 0; + const float kdtita = (cols_i-1)/fovh; - for (unsigned int u = 1; u < cols_i-1; u++) - if (null(u) == 0) - { - // Precomputed expressions - const float tw = weights(u); - const float tita = -0.5*fovh + u/kdtita; + //Fill the matrix A and the vector B + //The order of the variables will be (vx, vy, wz) - //Fill the matrix A - A(cont, 0) = tw*(cos(tita) + dtita(u)*kdtita*sin(tita)/range_inter[image_level](u)); - A(cont, 1) = tw*(sin(tita) - dtita(u)*kdtita*cos(tita)/range_inter[image_level](u)); - A(cont, 2) = tw*(-yy[image_level](u)*cos(tita) + xx[image_level](u)*sin(tita) - dtita(u)*kdtita); - B(cont,0) = tw*(-dt(u)); + for (unsigned int u = 1; u < cols_i-1; u++) + if (null(u) == 0) + { + // Precomputed expressions + const float tw = weights(u); + const float tita = -0.5*fovh + u/kdtita; - cont++; - } + //Fill the matrix A + A(cont, 0) = tw*(std::cos(tita) + dtita(u)*kdtita*std::sin(tita)/range_inter[image_level](u)); + A(cont, 1) = tw*(std::sin(tita) - dtita(u)*kdtita*std::cos(tita)/range_inter[image_level](u)); + A(cont, 2) = tw*(-yy[image_level](u)*std::cos(tita) + xx[image_level](u)*std::sin(tita) - dtita(u)*kdtita); + B(cont, 0) = tw*(-dt(u)); - //Solve the linear system of equations using a minimum least squares method + cont++; + } + + //Solve the linear system of equations using a minimum least squares method Eigen::MatrixXf AtA, AtB; - AtA.multiply_AtA(A); - AtB.multiply_AtB(A,B); - Var = AtA.ldlt().solve(AtB); + AtA = A.transpose()*A; + AtB = A.transpose()*B; + Var = AtA.ldlt().solve(AtB); - //Covariance matrix calculation Cov Order -> vx,vy,wz + //Covariance matrix calculation Cov Order -> vx,vy,wz Eigen::MatrixXf res(num_valid_range,1); - res = A*Var - B; - cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm(); + res = A*Var - B; + cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm(); - kai_loc_level = Var; + kai_loc_level_ = Var; } - // Solves the system by considering the Cauchy M-estimator robust-function void CLaserOdometry2D::solveSystemNonLinear() { - A.resize(num_valid_range,3); Aw.resize(num_valid_range,3); - B.setSize(num_valid_range,1); Bw.setSize(num_valid_range,1); - unsigned int cont = 0; - const float kdtita = float(cols_i-1)/fovh; + A.resize(num_valid_range, 3); Aw.resize(num_valid_range, 3); + B.resize(num_valid_range, 1); Bw.resize(num_valid_range, 1); + unsigned int cont = 0; + const float kdtita = float(cols_i-1)/fovh; - //Fill the matrix A and the vector B - //The order of the variables will be (vx, vy, wz) + //Fill the matrix A and the vector B + //The order of the variables will be (vx, vy, wz) - for (unsigned int u = 1; u < cols_i-1; u++) - if (null(u) == 0) - { - // Precomputed expressions - const float tw = weights(u); - const float tita = -0.5*fovh + u/kdtita; - - //Fill the matrix A - A(cont, 0) = tw*(cos(tita) + dtita(u)*kdtita*sin(tita)/range_inter[image_level](u)); - A(cont, 1) = tw*(sin(tita) - dtita(u)*kdtita*cos(tita)/range_inter[image_level](u)); - A(cont, 2) = tw*(-yy[image_level](u)*cos(tita) + xx[image_level](u)*sin(tita) - dtita(u)*kdtita); - B(cont,0) = tw*(-dt(u)); - - cont++; - } - - //Solve the linear system of equations using a minimum least squares method - 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 - Eigen::MatrixXf res(num_valid_range,1); - res = A*Var - B; - //cout << endl << "max res: " << res.maxCoeff(); - //cout << endl << "min res: " << res.minCoeff(); - - ////Compute the energy - //Compute the average dt - float aver_dt = 0.f, aver_res = 0.f; unsigned int ind = 0; - for (unsigned int u = 1; u < cols_i-1; u++) - if (null(u) == 0) - { - aver_dt += fabsf(dt(u)); - aver_res += fabsf(res(ind++)); - } - aver_dt /= cont; aver_res /= cont; -// printf("\n Aver dt = %f, aver res = %f", aver_dt, aver_res); - - - const float k = 10.f/aver_dt; //200 - //float energy = 0.f; - //for (unsigned int i=0; i vx,vy,wz + Eigen::MatrixXf res(num_valid_range,1); + res = A*Var - B; + //cout << endl << "max res: " << res.maxCoeff(); + //cout << endl << "min res: " << res.minCoeff(); + + ////Compute the energy + //Compute the average dt + float aver_dt = 0.f, aver_res = 0.f; unsigned int ind = 0; + for (unsigned int u = 1; u < cols_i-1; u++) + if (null(u) == 0) + { + aver_dt += std::abs(dt(u)); + aver_res += std::abs(res(ind++)); + } + aver_dt /= cont; aver_res /= cont; + // printf("\n Aver dt = %f, aver res = %f", aver_dt, aver_res); + + + const float k = 10.f/aver_dt; //200 + //float energy = 0.f; + //for (unsigned int i=0; i 0.f) - { - //Transform point to the warped reference frame - const float x_w = acu_trans(0,0)*xx[image_level](j) + acu_trans(0,1)*yy[image_level](j) + acu_trans(0,2); - const float y_w = acu_trans(1,0)*xx[image_level](j) + acu_trans(1,1)*yy[image_level](j) + acu_trans(1,2); - const float tita_w = atan2(y_w, x_w); - const float range_w = sqrt(x_w*x_w + y_w*y_w); + const float cols_lim = float(cols_i-1); + const float kdtita = cols_lim/fovh; - //Calculate warping - const float uwarp = kdtita*(tita_w + 0.5*fovh); + for (unsigned int j = 0; j 0.f) + { + //Transform point to the warped reference frame + const float x_w = acu_trans(0,0)*xx[image_level](j) + acu_trans(0,1)*yy[image_level](j) + acu_trans(0,2); + const float y_w = acu_trans(1,0)*xx[image_level](j) + acu_trans(1,1)*yy[image_level](j) + acu_trans(1,2); + const float tita_w = std::atan2(y_w, x_w); + const float range_w = std::sqrt(x_w*x_w + y_w*y_w); - //The warped pixel (which is not integer in general) contributes to all the surrounding ones - if (( uwarp >= 0.f)&&( uwarp < cols_lim)) - { - const int uwarp_l = uwarp; - const int uwarp_r = uwarp_l + 1; - const float delta_r = float(uwarp_r) - uwarp; - const float delta_l = uwarp - float(uwarp_l); + //Calculate warping + const float uwarp = kdtita*(tita_w + 0.5*fovh); - //Very close pixel - if (abs(round(uwarp) - uwarp) < 0.05f) - { - range_warped[image_level](round(uwarp)) += range_w; - wacu(round(uwarp)) += 1.f; - } - else - { - const float w_r = mrpt::math::square(delta_l); - range_warped[image_level](uwarp_r) += w_r*range_w; - wacu(uwarp_r) += w_r; + //The warped pixel (which is not integer in general) contributes to all the surrounding ones + if (( uwarp >= 0.f)&&( uwarp < cols_lim)) + { + const int uwarp_l = uwarp; + const int uwarp_r = uwarp_l + 1; + const float delta_r = float(uwarp_r) - uwarp; + const float delta_l = uwarp - float(uwarp_l); - const float w_l = mrpt::math::square(delta_r); - range_warped[image_level](uwarp_l) += w_l*range_w; - wacu(uwarp_l) += w_l; - } - } - } - } + //Very close pixel + if (std::abs(std::round(uwarp) - uwarp) < 0.05f) + { + range_warped[image_level](round(uwarp)) += range_w; + wacu(std::round(uwarp)) += 1.f; + } + else + { + const float w_r = delta_l*delta_l; + range_warped[image_level](uwarp_r) += w_r*range_w; + wacu(uwarp_r) += w_r; - //Scale the averaged range and compute coordinates - for (unsigned int u = 0; u 0.f) - { - const float tita = -0.5f*fovh + float(u)/kdtita; - range_warped[image_level](u) /= wacu(u); - xx_warped[image_level](u) = range_warped[image_level](u)*cos(tita); - yy_warped[image_level](u) = range_warped[image_level](u)*sin(tita); - } - else - { - range_warped[image_level](u) = 0.f; - xx_warped[image_level](u) = 0.f; - yy_warped[image_level](u) = 0.f; - } - } -} + const float w_l = delta_r*delta_r; + range_warped[image_level](uwarp_l) += w_l*range_w; + wacu(uwarp_l) += w_l; + } + } + } + } - - - - -void CLaserOdometry2D::filterLevelSolution() -{ - // Calculate Eigenvalues and Eigenvectors - //---------------------------------------------------------- - Eigen::SelfAdjointEigenSolver eigensolver(cov_odo); - if (eigensolver.info() != Eigen::Success) - { - ROS_INFO_COND(verbose, "[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 - //------------------------------------------------------------------------------------------------- - Eigen::Matrix Bii; - Eigen::Matrix 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 - //------------------------------------------------------------------------------------------------- - mrpt::math::CMatrixFloat31 kai_loc_sub; - - //Important: we have to substract the solutions from previous levels - Eigen::Matrix3f acu_trans; - acu_trans.setIdentity(); - for (unsigned int i=0; i 1.f) - kai_loc_sub(2) = 0.f; + //Scale the averaged range and compute coordinates + for (unsigned int u = 0; u 0.f) + { + const float tita = -0.5f*fovh + float(u)/kdtita; + range_warped[image_level](u) /= wacu(u); + xx_warped[image_level](u) = range_warped[image_level](u)*std::cos(tita); + yy_warped[image_level](u) = range_warped[image_level](u)*std::sin(tita); + } else { -#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 + range_warped[image_level](u) = 0.f; + xx_warped[image_level](u) = 0.f; + yy_warped[image_level](u) = 0.f; } - kai_loc_sub += kai_loc_old; - - Eigen::Matrix 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)); - - Eigen::Matrix 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); - //kai_b_fil_f(i,0) = (1.f*kai_b(i,0) + 0.f*kai_b_old_f(i,0))/(1.0f + 0.f); - } - - //Transform filtered speed to local reference frame and compute transformation - Eigen::Matrix kai_loc_fil = Bii.inverse().colPivHouseholderQr().solve(kai_b_fil); - - //transformation - const float incrx = kai_loc_fil(0)/fps; - const float incry = kai_loc_fil(1)/fps; - const float rot = kai_loc_fil(2)/fps; - transformations[level](0,0) = cos(rot); - transformations[level](0,1) = -sin(rot); - transformations[level](1,0) = sin(rot); - transformations[level](1,1) = cos(rot); - transformations[level](0,2) = incrx; - transformations[level](1,2) = incry; + } } +bool CLaserOdometry2D::filterLevelSolution() +{ + // Calculate Eigenvalues and Eigenvectors + //---------------------------------------------------------- + Eigen::SelfAdjointEigenSolver eigensolver(cov_odo); + if (eigensolver.info() != Eigen::Success) + { + ROS_WARN_COND(verbose, "[rf2o] ERROR: Eigensolver couldn't find a solution. Pose is not updated"); + return false; + } + + //First, we have to describe both the new linear and angular speeds in the "eigenvector" basis + //------------------------------------------------------------------------------------------------- + Eigen::Matrix Bii; + Eigen::Matrix kai_b; + Bii = eigensolver.eigenvectors(); + + kai_b = Bii.colPivHouseholderQr().solve(kai_loc_level_); + + assert((kai_loc_level_).isApprox(Bii*kai_b, 1e-5) && "Ax=b has no solution." && __LINE__); + + //Second, we have to describe both the old linear and angular speeds in the "eigenvector" basis too + //------------------------------------------------------------------------------------------------- + MatrixS31 kai_loc_sub; + + //Important: we have to substract the solutions from previous levels + Eigen::Matrix3f acu_trans; + acu_trans.setIdentity(); + for (unsigned int i=0; i 1.f) + kai_loc_sub(2) = 0.f; + else + { + kai_loc_sub(2) = -fps*std::acos(acu_trans(0,0))*rf2o::sign(acu_trans(1,0)); + } + kai_loc_sub += kai_loc_old_; + + Eigen::Matrix kai_b_old; + kai_b_old = Bii.colPivHouseholderQr().solve(kai_loc_sub); + + assert((kai_loc_sub).isApprox(Bii*kai_b_old, 1e-5) && "Ax=b has no solution." && __LINE__); + + //Filter speed + const float cf = 15e3f*std::exp(-float(int(level))), + df = 0.05f*std::exp(-float(int(level))); + + Eigen::Matrix kai_b_fil; + for (unsigned int i=0; i<3; i++) + { + kai_b_fil(i) = (kai_b(i) + (cf*eigensolver.eigenvalues()(i,0) + df)*kai_b_old(i))/(1.f + cf*eigensolver.eigenvalues()(i,0) + df); + //kai_b_fil_f(i,0) = (1.f*kai_b(i,0) + 0.f*kai_b_old_f(i,0))/(1.0f + 0.f); + } + + //Transform filtered speed to local reference frame and compute transformation + Eigen::Matrix kai_loc_fil = Bii.inverse().colPivHouseholderQr().solve(kai_b_fil); + + assert((kai_b_fil).isApprox(Bii.inverse()*kai_loc_fil, 1e-5) && "Ax=b has no solution." && __LINE__); + + //transformation + const float incrx = kai_loc_fil(0)/fps; + const float incry = kai_loc_fil(1)/fps; + const float rot = kai_loc_fil(2)/fps; + + transformations[level](0,0) = std::cos(rot); + transformations[level](0,1) = -std::sin(rot); + transformations[level](1,0) = std::sin(rot); + transformations[level](1,1) = std::cos(rot); + transformations[level](0,2) = incrx; + transformations[level](1,2) = incry; + + return true; +} void CLaserOdometry2D::PoseUpdate() { - //First, compute the overall transformation - //--------------------------------------------------- + // First, compute the overall transformation + //--------------------------------------------------- Eigen::Matrix3f acu_trans; - acu_trans.setIdentity(); - for (unsigned int i=1; i<=ctf_levels; i++) - acu_trans = transformations[i-1]*acu_trans; + acu_trans.setIdentity(); + + for (unsigned int i=1; i<=ctf_levels; i++) + acu_trans = transformations[i-1]*acu_trans; + + // Compute kai_loc and kai_abs + //-------------------------------------------------------- + kai_loc_(0) = fps*acu_trans(0,2); + kai_loc_(1) = fps*acu_trans(1,2); + + if (acu_trans(0,0) > 1.f) + kai_loc_(2) = 0.f; + else + { + kai_loc_(2) = fps*std::acos(acu_trans(0,0))*rf2o::sign(acu_trans(1,0)); + } + + //cout << endl << "Arc cos (incr tita): " << kai_loc_(2); + + float phi = rf2o::getYaw(laser_pose_.rotation()); + + kai_abs_(0) = kai_loc_(0)*std::cos(phi) - kai_loc_(1)*std::sin(phi); + kai_abs_(1) = kai_loc_(0)*std::sin(phi) + kai_loc_(1)*std::cos(phi); + kai_abs_(2) = kai_loc_(2); - // Compute kai_loc and kai_abs - //-------------------------------------------------------- - kai_loc(0) = fps*acu_trans(0,2); - kai_loc(1) = fps*acu_trans(1,2); - if (acu_trans(0,0) > 1.f) - kai_loc(2) = 0.f; - else - { -#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); + // Update poses + //------------------------------------------------------- + laser_oldpose_ = laser_pose_; - float phi = laser_pose.yaw(); + // Eigen::Matrix3f aux_acu = acu_trans; + Pose3d pose_aux_2D = Pose3d::Identity(); - kai_abs(0) = kai_loc(0)*cos(phi) - kai_loc(1)*sin(phi); - kai_abs(1) = kai_loc(0)*sin(phi) + kai_loc(1)*cos(phi); - kai_abs(2) = kai_loc(2); + pose_aux_2D = rf2o::matrixYaw(double(kai_loc_(2)/fps)); + pose_aux_2D.translation()(0) = acu_trans(0,2); + pose_aux_2D.translation()(1) = acu_trans(1,2); + laser_pose_ = laser_pose_ * pose_aux_2D; - // Update poses - //------------------------------------------------------- - laser_oldpose = laser_pose; + last_increment_ = pose_aux_2D; - mrpt::math::CMatrixDouble33 aux_acu = acu_trans; - 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); + // Compute kai_loc_old + //------------------------------------------------------- + phi = rf2o::getYaw(laser_pose_.rotation()); + kai_loc_old_(0) = kai_abs_(0)*std::cos(phi) + kai_abs_(1)*std::sin(phi); + kai_loc_old_(1) = -kai_abs_(0)*std::sin(phi) + kai_abs_(1)*std::cos(phi); + kai_loc_old_(2) = kai_abs_(2); - last_increment = pose_aux_2D; + ROS_INFO_COND(verbose, "[rf2o] LASERodom = [%f %f %f]", + laser_pose_.translation()(0), + laser_pose_.translation()(1), + rf2o::getYaw(laser_pose_.rotation())); + //Compose Transformations + robot_pose_ = laser_pose_ * laser_pose_on_robot_inv_; + ROS_INFO_COND(verbose, "BASEodom = [%f %f %f]", + robot_pose_.translation()(0), + robot_pose_.translation()(1), + rf2o::getYaw(robot_pose_.rotation())); - // Compute kai_loc_old - //------------------------------------------------------- - phi = laser_pose.yaw(); - kai_loc_old(0) = kai_abs(0)*cos(phi) + kai_abs(1)*sin(phi); - kai_loc_old(1) = -kai_abs(0)*sin(phi) + kai_abs(1)*cos(phi); - kai_loc_old(2) = kai_abs(2); + // 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 = (current_scan_time - last_odom_time).toSec(); + last_odom_time = current_scan_time; + lin_speed = acu_trans(0,2) / 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; - ROS_INFO_COND(verbose, "[rf2o] LASERodom = [%f %f %f]",laser_pose.x(),laser_pose.y(),laser_pose.yaw()); + double ang_inc = rf2o::getYaw(robot_pose_.rotation()) - + rf2o::getYaw(robot_oldpose_.rotation()); - //Compose Transformations - robot_pose = laser_pose + laser_pose_on_robot_inv; - ROS_INFO_COND(verbose, "BASEodom = [%f %f %f]",robot_pose.x(),robot_pose.y(),robot_pose.yaw()); + if (ang_inc > 3.14159) + ang_inc -= 2*3.14159; + if (ang_inc < -3.14159) + ang_inc += 2*3.14159; - // 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 = (current_scan_time - last_odom_time).toSec(); - last_odom_time = current_scan_time; - lin_speed = acu_trans(0,2) / 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; - ang_speed = ang_inc/time_inc_sec; - robot_oldpose = robot_pose; + ang_speed = ang_inc/time_inc_sec; + robot_oldpose_ = robot_pose_; - //filter speeds - /* + //filter speeds + /* last_m_lin_speeds.push_back(lin_speed); if (last_m_lin_speeds.size()>4) last_m_lin_speeds.erase(last_m_lin_speeds.begin()); @@ -959,3 +982,5 @@ void CLaserOdometry2D::PoseUpdate() ang_speed = sum2 / last_m_ang_speeds.size(); */ } + +} /* namespace rf2o */ diff --git a/src/CLaserOdometry2DNode.cpp b/src/CLaserOdometry2DNode.cpp index 1310a8e..942c000 100644 --- 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 #include +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("laser_scan_topic",laser_scan_topic,"/laser_scan"); - pn.param("odom_topic", odom_topic, "/odom_rf2o"); - pn.param("base_frame_id", base_frame_id, "/base_link"); - pn.param("odom_frame_id", odom_frame_id, "/odom"); - pn.param("publish_tf", publish_tf, true); - pn.param("init_pose_from_topic", init_pose_from_topic, "/base_pose_ground_truth"); - pn.param("freq",freq,10.0); - pn.param("verbose", verbose, true); + //Read Parameters + //---------------- + ros::NodeHandle pn("~"); + pn.param("laser_scan_topic",laser_scan_topic,"/laser_scan"); + pn.param("odom_topic", odom_topic, "/odom_rf2o"); + pn.param("base_frame_id", base_frame_id, "/base_link"); + pn.param("odom_frame_id", odom_frame_id, "/odom"); + pn.param("publish_tf", publish_tf, true); + pn.param("init_pose_from_topic", init_pose_from_topic, "/base_pose_ground_truth"); + pn.param("freq",freq,10.0); + pn.param("verbose", verbose, true); - //Publishers and Subscribers - //-------------------------- - odom_pub = pn.advertise(odom_topic, 5); - laser_sub = n.subscribe(laser_scan_topic,1,&CLaserOdometry2DNode::LaserCallBack,this); + //Publishers and Subscribers + //-------------------------- + odom_pub = pn.advertise(odom_topic, 5); + laser_sub = n.subscribe(laser_scan_topic,1,&CLaserOdometry2DNode::LaserCallBack,this); - //init pose?? - if (init_pose_from_topic != "") - { - initPose_sub = n.subscribe(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(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; iranges[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; iranges[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; }