mirrors/rf2o_laser_odometry
1
0
Fork 0
mirror of https://github.com/MAPIRlab/rf2o_laser_odometry.git synced 2023-04-06 19:28:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

replace mrpt::math::CMatrixFloat31 with MatrixS31

Browse Source
This commit is contained in:
Jeremie Deray
2017-08-15 13:22:56 +02:00
parent 235d8c6fdf
commit f84885b47d
2 changed files with 28 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
include/rf2o_laser_odometry/CLaserOdometry2D.h
View File

@@ -89,7 +89,7 @@ public:
using Pose2d = Eigen::Isometry2d; using Pose2d = Eigen::Isometry2d;
using Pose3d = Eigen::Isometry3d; using Pose3d = Eigen::Isometry3d;
using MatrixF31 = Eigen::Matrix<Scalar, 3, 1>; using MatrixS31 = Eigen::Matrix<Scalar, 3, 1>;
using IncrementCov = Eigen::Matrix<Scalar, 3, 3>; using IncrementCov = Eigen::Matrix<Scalar, 3, 3>;
CLaserOdometry2D(); CLaserOdometry2D();
@@ -140,8 +140,9 @@ protected:
Eigen::MatrixXf A,Aw; Eigen::MatrixXf A,Aw;
Eigen::MatrixXf B,Bw; Eigen::MatrixXf B,Bw;
Eigen::Matrix<float, 3, 1> Var; //3 unknowns: vx, vy, w
Eigen::Matrix<float, 3, 3> cov_odo; MatrixS31 Var; //3 unknowns: vx, vy, w
IncrementCov cov_odo;
@@ -164,10 +165,10 @@ protected:
float m_runtime; float m_runtime;
ros::Time last_odom_time, current_scan_time; ros::Time last_odom_time, current_scan_time;
mrpt::math::CMatrixFloat31 kai_abs; MatrixS31 kai_abs_;
mrpt::math::CMatrixFloat31 kai_loc; MatrixS31 kai_loc_;
mrpt::math::CMatrixFloat31 kai_loc_old; MatrixS31 kai_loc_old_;
mrpt::math::CMatrixFloat31 kai_loc_level; MatrixS31 kai_loc_level_;
Pose3d last_increment_; Pose3d last_increment_;
Pose3d laser_pose_on_robot_; Pose3d laser_pose_on_robot_;

40
src/CLaserOdometry2D.cpp
View File

@@ -138,8 +138,8 @@ void CLaserOdometry2D::init(const sensor_msgs::LaserScan& scan,
//Compute gaussian mask //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]; 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_abs_ = MatrixS31::Zero();
kai_loc_old.assign(0.f); kai_loc_old_ = MatrixS31::Zero();
module_initialized = true; module_initialized = true;
last_odom_time = ros::Time::now(); last_odom_time = ros::Time::now();
@@ -603,7 +603,7 @@ void CLaserOdometry2D::solveSystemOneLevel()
res = A*Var - B; res = A*Var - B;
cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm(); cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm();
kai_loc_level = Var; kai_loc_level_ = Var;
} }
@@ -698,9 +698,9 @@ void CLaserOdometry2D::solveSystemNonLinear()
} }
cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm(); cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm();
kai_loc_level = Var; kai_loc_level_ = Var;
ROS_INFO_STREAM_COND(verbose, "[rf2o] COV_ODO: " << cov_odo); ROS_INFO_STREAM_COND(verbose, "[rf2o] COV_ODO:\n" << cov_odo);
} }
void CLaserOdometry2D::Reset(const Pose3d& ini_pose, CObservation2DRangeScan scan) void CLaserOdometry2D::Reset(const Pose3d& ini_pose, CObservation2DRangeScan scan)
@@ -812,11 +812,11 @@ void CLaserOdometry2D::filterLevelSolution()
Eigen::Matrix<float,3,1> kai_b; Eigen::Matrix<float,3,1> kai_b;
Bii = eigensolver.eigenvectors(); Bii = eigensolver.eigenvectors();
kai_b = Bii.colPivHouseholderQr().solve(kai_loc_level); 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 //Second, we have to describe both the old linear and angular speeds in the "eigenvector" basis too
//------------------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------------------
mrpt::math::CMatrixFloat31 kai_loc_sub; MatrixS31 kai_loc_sub;
//Important: we have to substract the solutions from previous levels //Important: we have to substract the solutions from previous levels
Eigen::Matrix3f acu_trans; Eigen::Matrix3f acu_trans;
@@ -832,7 +832,7 @@ void CLaserOdometry2D::filterLevelSolution()
{ {
kai_loc_sub(2) = -fps*std::acos(acu_trans(0,0))*rf2o::sign(acu_trans(1,0)); kai_loc_sub(2) = -fps*std::acos(acu_trans(0,0))*rf2o::sign(acu_trans(1,0));
} }
kai_loc_sub += kai_loc_old; kai_loc_sub += kai_loc_old_;
Eigen::Matrix<float,3,1> kai_b_old; Eigen::Matrix<float,3,1> kai_b_old;
kai_b_old = Bii.colPivHouseholderQr().solve(kai_loc_sub); kai_b_old = Bii.colPivHouseholderQr().solve(kai_loc_sub);
@@ -876,21 +876,21 @@ void CLaserOdometry2D::PoseUpdate()
// Compute kai_loc and kai_abs // Compute kai_loc and kai_abs
//-------------------------------------------------------- //--------------------------------------------------------
kai_loc(0) = fps*acu_trans(0,2); kai_loc_(0) = fps*acu_trans(0,2);
kai_loc(1) = fps*acu_trans(1,2); kai_loc_(1) = fps*acu_trans(1,2);
if (acu_trans(0,0) > 1.f) if (acu_trans(0,0) > 1.f)
kai_loc(2) = 0.f; kai_loc_(2) = 0.f;
else else
{ {
kai_loc(2) = fps*acos(acu_trans(0,0))*rf2o::sign(acu_trans(1,0)); 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); //cout << endl << "Arc cos (incr tita): " << kai_loc_(2);
float phi = rf2o::getYaw(laser_pose_.rotation()); float phi = rf2o::getYaw(laser_pose_.rotation());
kai_abs(0) = kai_loc(0)*std::cos(phi) - kai_loc(1)*std::sin(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_(1) = kai_loc_(0)*std::sin(phi) + kai_loc_(1)*std::cos(phi);
kai_abs(2) = kai_loc(2); kai_abs_(2) = kai_loc_(2);
// Update poses // Update poses
@@ -900,7 +900,7 @@ void CLaserOdometry2D::PoseUpdate()
mrpt::math::CMatrixDouble33 aux_acu = acu_trans; mrpt::math::CMatrixDouble33 aux_acu = acu_trans;
Pose3d pose_aux_2D = Pose3d::Identity(); Pose3d pose_aux_2D = Pose3d::Identity();
pose_aux_2D = rf2o::matrixYaw(double(kai_loc(2)/fps)); pose_aux_2D = rf2o::matrixYaw(double(kai_loc_(2)/fps));
pose_aux_2D.translation()(0) = acu_trans(0,2); pose_aux_2D.translation()(0) = acu_trans(0,2);
pose_aux_2D.translation()(1) = acu_trans(1,2); pose_aux_2D.translation()(1) = acu_trans(1,2);
@@ -913,9 +913,9 @@ void CLaserOdometry2D::PoseUpdate()
// Compute kai_loc_old // Compute kai_loc_old
//------------------------------------------------------- //-------------------------------------------------------
phi = rf2o::getYaw(laser_pose_.rotation()); 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_(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_(1) = -kai_abs_(0)*std::sin(phi) + kai_abs_(1)*std::cos(phi);
kai_loc_old(2) = kai_abs(2); kai_loc_old_(2) = kai_abs_(2);
ROS_INFO_COND(verbose, "[rf2o] LASERodom = [%f %f %f]", ROS_INFO_COND(verbose, "[rf2o] LASERodom = [%f %f %f]",
laser_pose_.translation()(0), laser_pose_.translation()(0),