From 235d8c6fdfe9d5ca58661aba67afcf76e36d6859 Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Tue, 15 Aug 2017 13:05:19 +0200 Subject: [PATCH] replace mrpt::poses::CPose3D with Eigen::Isometry3d add some std namespace. --- .../rf2o_laser_odometry/CLaserOdometry2D.h | 72 ++++-- src/CLaserOdometry2D.cpp | 228 +++++++++--------- src/CLaserOdometry2DNode.cpp | 42 ++-- 3 files changed, 191 insertions(+), 151 deletions(-) diff --git a/include/rf2o_laser_odometry/CLaserOdometry2D.h b/include/rf2o_laser_odometry/CLaserOdometry2D.h index 6985391..6cbf9e2 100644 --- a/include/rf2o_laser_odometry/CLaserOdometry2D.h +++ b/include/rf2o_laser_odometry/CLaserOdometry2D.h @@ -45,34 +45,71 @@ #include #include +#include #include #include #include #include +namespace rf2o { +template +inline int sign(T x) { return x<0 ? -1:1; } +template +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); +} +} // namespace rf2o class CLaserOdometry2D { public: - CLaserOdometry2D(); - ~CLaserOdometry2D(); - void Init(const sensor_msgs::LaserScan& scan, + using Scalar = float; + + using Pose2d = Eigen::Isometry2d; + using Pose3d = Eigen::Isometry3d; + using MatrixF31 = Eigen::Matrix; + using IncrementCov = Eigen::Matrix; + + CLaserOdometry2D(); + virtual ~CLaserOdometry2D() = default; + + void init(const sensor_msgs::LaserScan& scan, const geometry_msgs::Pose& initial_robot_pose); bool is_initialized(); - void odometryCalculation(const sensor_msgs::LaserScan& scan); + bool odometryCalculation(const sensor_msgs::LaserScan& scan); - void setLaserPose(const mrpt::poses::CPose3D& laser_pose); + void setLaserPose(const Pose3d& laser_pose); - const mrpt::poses::CPose3D& getIncrement() const; + const Pose3d& getIncrement() const; - const Eigen::Matrix& getIncrementCovariance() const; + const IncrementCov& getIncrementCovariance() const; - mrpt::poses::CPose3D& getPose(); - const mrpt::poses::CPose3D& getPose() const; + Pose3d& getPose(); + const Pose3d& getPose() const; protected: @@ -132,15 +169,14 @@ protected: mrpt::math::CMatrixFloat31 kai_loc_old; mrpt::math::CMatrixFloat31 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; - - 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; @@ -158,7 +194,7 @@ protected: void solveSystemNonLinear(); void filterLevelSolution(); void PoseUpdate(); - void Reset(mrpt::poses::CPose3D ini_pose, CObservation2DRangeScan scan); + void Reset(const Pose3d& ini_pose, CObservation2DRangeScan scan); }; #endif diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp index 12c4c5b..7157b52 100644 --- a/src/CLaserOdometry2D.cpp +++ b/src/CLaserOdometry2D.cpp @@ -26,19 +26,12 @@ CLaserOdometry2D::CLaserOdometry2D() : // } - -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() @@ -46,7 +39,7 @@ bool CLaserOdometry2D::is_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 @@ -59,26 +52,20 @@ void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan, 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 robotInitialPose = Pose3d::Identity(); + const geometry_msgs::Pose _src = initial_robot_pose; - robotInitialPose.x(_src.position.x); - robotInitialPose.y(_src.position.y); + robotInitialPose = Eigen::Quaterniond(_src.orientation.w, + _src.orientation.x, + _src.orientation.y, + _src.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); + robotInitialPose.translation()(0) = _src.position.x; + robotInitialPose.translation()(1) = _src.position.y; //Set the initial pose - laser_pose = robotInitialPose + laser_pose_on_robot; - laser_oldpose = robotInitialPose + laser_pose_on_robot; + laser_pose_ = robotInitialPose * laser_pose_on_robot_; + laser_oldpose_ = laser_oldpose_; // Init module (internal) //------------------------ @@ -91,7 +78,7 @@ void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan, //Resize pyramid unsigned int s, cols_i; - const unsigned int pyr_levels = round(log2(round(float(width) / float(cols)))) + ctf_levels; + 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); @@ -158,9 +145,9 @@ void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan, 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,17 +155,17 @@ 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 @@ -198,9 +185,9 @@ void CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan) transformations[i].setIdentity(); level = i; - unsigned int s = pow(2.f,int(ctf_levels-(i+1))); - cols_i = ceil(float(cols)/float(s)); - image_level = ctf_levels - i + round(log2(round(float(width)/float(cols)))) - 1; + unsigned int s = std::pow(2.f,int(ctf_levels-(i+1))); + cols_i = std::ceil(float(cols)/float(s)); + image_level = ctf_levels - i + std::round(std::log2(std::round(float(width)/float(cols)))) - 1; //1. Perform warping if (i == 0) @@ -244,6 +231,8 @@ void CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan) //Update poses PoseUpdate(); + + return true; } @@ -259,13 +248,13 @@ void CLaserOdometry2D::createImagePyramid() //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=0)&&(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); + xx[i](u) = range[i](u)*std::cos(tita); + yy[i](u) = range[i](u)*std::sin(tita); } else { @@ -449,15 +438,21 @@ void CLaserOdometry2D::calculaterangeDerivativesSurface() 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)); + 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; + if (dist > 0.f) - rtita(u) = sqrt(dist); + rtita(u) = std::sqrt(dist); } //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(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); @@ -503,7 +498,7 @@ void CLaserOdometry2D::computeNormals() if (null(u) == 0.f) { const float tita = -0.5f*fovh + float(u)*incr_tita; - const float alfa = -atan2(2.f*dtita(u), 2.f*range[image_level](u)*incr_tita); + const float alfa = -std::atan2(2.f*dtita(u), 2.f*range[image_level](u)*incr_tita); norm_ang(u) = tita + alfa; if (norm_ang(u) < -M_PI) norm_ang(u) += 2.f*M_PI; @@ -512,8 +507,8 @@ void CLaserOdometry2D::computeNormals() else if (norm_ang(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); } } } @@ -526,7 +521,7 @@ void CLaserOdometry2D::computeWeights() //Parameters for error_linearization const float kdtita = 1.f; - const float kdt = kdtita/mrpt::math::square(fps); + const float kdt = kdtita / (fps*fps); const float k2d = 0.2f; for (unsigned int u = 1; u < cols_i-1; u++) @@ -540,9 +535,11 @@ void CLaserOdometry2D::computeWeights() 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(); @@ -587,10 +584,10 @@ void CLaserOdometry2D::solveSystemOneLevel() 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)); + 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)); cont++; } @@ -629,10 +626,10 @@ void CLaserOdometry2D::solveSystemNonLinear() 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)); + 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)); cont++; } @@ -655,8 +652,8 @@ void CLaserOdometry2D::solveSystemNonLinear() 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 += 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); @@ -677,13 +674,13 @@ void CLaserOdometry2D::solveSystemNonLinear() for (unsigned int u = 1; u < cols_i-1; u++) if (null(u) == 0) { - const float res_weight = sqrt(1.f/(1.f + mrpt::math::square(k*res(cont)))); + const float res_weight = std::sqrt(1.f/(1.f + ((k*res(cont))*(k*res(cont))))); //Fill the matrix Aw Aw(cont,0) = res_weight*A(cont,0); Aw(cont,1) = res_weight*A(cont,1); Aw(cont,2) = res_weight*A(cont,2); - Bw(cont) = res_weight*B(cont); + Bw(cont) = res_weight*B(cont); cont++; } @@ -706,11 +703,11 @@ void CLaserOdometry2D::solveSystemNonLinear() ROS_INFO_STREAM_COND(verbose, "[rf2o] COV_ODO: " << cov_odo); } -void CLaserOdometry2D::Reset(mrpt::poses::CPose3D ini_pose, CObservation2DRangeScan scan) +void CLaserOdometry2D::Reset(const Pose3d& ini_pose, CObservation2DRangeScan scan) { //Set the initial pose - laser_pose = ini_pose; - laser_oldpose = ini_pose; + laser_pose_ = ini_pose; + laser_oldpose_ = ini_pose; //readLaser(scan); createImagePyramid(); @@ -741,8 +738,8 @@ void CLaserOdometry2D::performWarping() //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 tita_w = std::atan2(y_w, x_w); + const float range_w = std::sqrt(x_w*x_w + y_w*y_w); //Calculate warping const float uwarp = kdtita*(tita_w + 0.5*fovh); @@ -756,18 +753,18 @@ void CLaserOdometry2D::performWarping() const float delta_l = uwarp - float(uwarp_l); //Very close pixel - if (abs(round(uwarp) - uwarp) < 0.05f) + if (std::abs(std::round(uwarp) - uwarp) < 0.05f) { range_warped[image_level](round(uwarp)) += range_w; - wacu(round(uwarp)) += 1.f; + wacu(std::round(uwarp)) += 1.f; } else { - const float w_r = mrpt::math::square(delta_l); + const float w_r = delta_l*delta_l; range_warped[image_level](uwarp_r) += w_r*range_w; wacu(uwarp_r) += w_r; - const float w_l = mrpt::math::square(delta_r); + const float w_l = delta_r*delta_r; range_warped[image_level](uwarp_l) += w_l*range_w; wacu(uwarp_l) += w_l; } @@ -782,8 +779,8 @@ void CLaserOdometry2D::performWarping() { 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); + 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 { @@ -833,11 +830,7 @@ void CLaserOdometry2D::filterLevelSolution() kai_loc_sub(2) = 0.f; 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 + kai_loc_sub(2) = -fps*std::acos(acu_trans(0,0))*rf2o::sign(acu_trans(1,0)); } kai_loc_sub += kai_loc_old; @@ -845,7 +838,8 @@ void CLaserOdometry2D::filterLevelSolution() kai_b_old = Bii.colPivHouseholderQr().solve(kai_loc_sub); //Filter speed - const float cf = 15e3f*expf(-int(level)), df = 0.05f*expf(-int(level)); + 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++) @@ -861,10 +855,10 @@ void CLaserOdometry2D::filterLevelSolution() 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,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; } @@ -888,45 +882,53 @@ void CLaserOdometry2D::PoseUpdate() 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 + kai_loc(2) = fps*acos(acu_trans(0,0))*rf2o::sign(acu_trans(1,0)); } //cout << endl << "Arc cos (incr tita): " << kai_loc(2); - float phi = laser_pose.yaw(); + float phi = rf2o::getYaw(laser_pose_.rotation()); - 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(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); // Update poses //------------------------------------------------------- - laser_oldpose = laser_pose; + laser_oldpose_ = laser_pose_; 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); + Pose3d pose_aux_2D = Pose3d::Identity(); - last_increment = pose_aux_2D; + 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; + + last_increment_ = pose_aux_2D; // 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); + 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); - ROS_INFO_COND(verbose, "[rf2o] LASERodom = [%f %f %f]",laser_pose.x(),laser_pose.y(),laser_pose.yaw()); + ROS_INFO_COND(verbose, "[rf2o] LASERodom = [%f %f %f]", + laser_pose_.translation()(0), + laser_pose_.translation()(1), + 0/*TODOlaser_pose_.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()); + 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), + 0/*TODOrobot_pose_.rotation()*/); // Estimate linear/angular speeds (mandatory for base_local_planner) // last_scan -> the last scan received @@ -936,13 +938,17 @@ void CLaserOdometry2D::PoseUpdate() 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(); + + double ang_inc = rf2o::getYaw(robot_pose_.rotation()) - + rf2o::getYaw(robot_oldpose_.rotation()); + 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; + robot_oldpose_ = robot_pose_; //filter speeds /* diff --git a/src/CLaserOdometry2DNode.cpp b/src/CLaserOdometry2DNode.cpp index 1310a8e..dc73281 100644 --- a/src/CLaserOdometry2DNode.cpp +++ b/src/CLaserOdometry2DNode.cpp @@ -23,7 +23,7 @@ class CLaserOdometry2DNode : CLaserOdometry2D public: CLaserOdometry2DNode(); - ~CLaserOdometry2DNode(); + ~CLaserOdometry2DNode() = default; void process(const ros::TimerEvent &); void publish(); @@ -32,7 +32,7 @@ public: public: - bool publish_tf,new_scan_available; + bool publish_tf, new_scan_available; double freq; @@ -109,11 +109,6 @@ CLaserOdometry2DNode::CLaserOdometry2DNode() : ROS_INFO_STREAM("Listening laser scan from topic: " << laser_sub.getTopic()); } -CLaserOdometry2DNode::~CLaserOdometry2DNode() -{ - // -} - bool CLaserOdometry2DNode::setLaserPoseFromTf() { bool retrieved = false; @@ -134,18 +129,21 @@ bool CLaserOdometry2DNode::setLaserPoseFromTf() 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); + + 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); @@ -194,7 +192,7 @@ void CLaserOdometry2DNode::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& } else { - Init(last_scan, initial_robot_pose.pose.pose); + init(last_scan, initial_robot_pose.pose.pose); first_laser_scan = false; } } @@ -221,10 +219,10 @@ void CLaserOdometry2DNode::publish() 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.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(robot_pose.yaw()); + odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(rf2o::getYaw(robot_pose_.rotation())); //send the transform odom_broadcaster.sendTransform(odom_trans); } @@ -236,10 +234,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