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

tracking & cheap handling of uninitialized values issue

Browse Source
This commit is contained in:
Jeremie Deray
2017-08-19 18:50:28 +02:00
committed by Jeremie Deray
parent fb5334d542
commit 02421bac19
2 changed files with 73 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
include/rf2o_laser_odometry/CLaserOdometry2D.h
View File

@@ -36,7 +36,7 @@
namespace rf2o { namespace rf2o {
template <typename T> template <typename T>
inline int sign(T x) { return x<0 ? -1:1; } inline T sign(const T x) { return x<T(0) ? -1:1; }
template <typename Derived> template <typename Derived>
inline typename Eigen::MatrixBase<Derived>::Scalar inline typename Eigen::MatrixBase<Derived>::Scalar
@@ -170,7 +170,7 @@ protected:
void findNullPoints(); void findNullPoints();
void solveSystemOneLevel(); void solveSystemOneLevel();
void solveSystemNonLinear(); void solveSystemNonLinear();
void filterLevelSolution(); bool filterLevelSolution();
void PoseUpdate(); void PoseUpdate();
void Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan scan*/); void Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan scan*/);
}; };

113
src/CLaserOdometry2D.cpp
View File

@@ -24,8 +24,16 @@ namespace rf2o {
//--------------------------------------------- //---------------------------------------------
CLaserOdometry2D::CLaserOdometry2D() : CLaserOdometry2D::CLaserOdometry2D() :
verbose(false),
module_initialized(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())
{ {
// //
} }
@@ -34,7 +42,7 @@ void CLaserOdometry2D::setLaserPose(const Pose3d& laser_pose)
{ {
//Set laser pose on the robot //Set laser pose on the robot
laser_pose_on_robot_ = laser_pose; laser_pose_on_robot_ = laser_pose;
laser_pose_on_robot_inv_ = laser_pose_on_robot_.inverse(); laser_pose_on_robot_inv_ = laser_pose_on_robot_.inverse();
} }
@@ -51,24 +59,26 @@ void CLaserOdometry2D::init(const sensor_msgs::LaserScan& scan,
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 cols = width; // Max resolution. Should be similar to the width parameter
fovh = fabs(scan.angle_max - scan.angle_min); // Horizontal Laser's FOV fovh = std::abs(scan.angle_max - scan.angle_min); // Horizontal Laser's FOV
ctf_levels = 5; // Coarse-to-Fine levels ctf_levels = 5; // Coarse-to-Fine levels
iter_irls = 5; //Num iterations to solve iterative reweighted least squares iter_irls = 5; //Num iterations to solve iterative reweighted least squares
Pose3d robotInitialPose = Pose3d::Identity(); Pose3d robot_initial_pose = Pose3d::Identity();
const geometry_msgs::Pose _src = initial_robot_pose;
robotInitialPose = Eigen::Quaterniond(_src.orientation.w, robot_initial_pose = Eigen::Quaterniond(initial_robot_pose.orientation.w,
_src.orientation.x, initial_robot_pose.orientation.x,
_src.orientation.y, initial_robot_pose.orientation.y,
_src.orientation.z); initial_robot_pose.orientation.z);
robotInitialPose.translation()(0) = _src.position.x; robot_initial_pose.translation()(0) = initial_robot_pose.position.x;
robotInitialPose.translation()(1) = _src.position.y; robot_initial_pose.translation()(1) = initial_robot_pose.position.y;
ROS_INFO_STREAM_COND(verbose, "[rf2o] Setting origin at:\n"
<< robot_initial_pose.matrix());
//Set the initial pose //Set the initial pose
laser_pose_ = robotInitialPose * laser_pose_on_robot_; laser_pose_ = robot_initial_pose * laser_pose_on_robot_;
laser_oldpose_ = laser_oldpose_; laser_oldpose_ = laser_oldpose_;
// Init module (internal) // Init module (internal)
@@ -129,16 +139,20 @@ void CLaserOdometry2D::init(const sensor_msgs::LaserScan& scan,
norm_ang.resize(1, cols); norm_ang.resize(1, cols);
weights.resize(1, cols); weights.resize(1, cols);
null = Eigen::MatrixXi::Constant(1, cols, 0); null = Eigen::MatrixXi::Constant(1, cols, 0);
cov_odo = IncrementCov::Zero(); cov_odo = IncrementCov::Zero();
fps = 1.f; //In Hz fps = 1.f; //In Hz
num_valid_range = 0; num_valid_range = 0;
//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_ = MatrixS31::Zero(); kai_abs_ = MatrixS31::Zero();
kai_loc_old_ = MatrixS31::Zero(); kai_loc_old_ = MatrixS31::Zero();
module_initialized = true; module_initialized = true;
@@ -172,8 +186,7 @@ bool CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan)
//================================================================================== //==================================================================================
//copy laser scan to internal variable //copy laser scan to internal variable
for (unsigned int i = 0; i<width; i++) range_wf = Eigen::Map<const Eigen::MatrixXf>(scan.ranges.data(), width, 1);
range_wf(i) = scan.ranges[i];
ros::WallTime start = ros::WallTime::now(); ros::WallTime start = ros::WallTime::now();
@@ -194,8 +207,8 @@ bool CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan)
if (i == 0) if (i == 0)
{ {
range_warped[image_level] = range[image_level]; range_warped[image_level] = range[image_level];
xx_warped[image_level] = xx[image_level]; xx_warped[image_level] = xx[image_level];
yy_warped[image_level] = yy[image_level]; yy_warped[image_level] = yy[image_level];
} }
else else
performWarping(); performWarping();
@@ -221,10 +234,19 @@ bool CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan)
solveSystemNonLinear(); solveSystemNonLinear();
//solveSystemOneLevel(); //without robust-function //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;
}
//8. Filter solution //8. Filter solution
filterLevelSolution(); if (!filterLevelSolution()) return false;
} }
m_runtime = ros::WallTime::now() - start; m_runtime = ros::WallTime::now() - start;
@@ -289,7 +311,6 @@ void CLaserOdometry2D::createImagePyramid()
} }
else else
range[i](u) = 0.f; range[i](u) = 0.f;
} }
//Boundary //Boundary
@@ -318,7 +339,6 @@ void CLaserOdometry2D::createImagePyramid()
} }
else else
range[i](u) = 0.f; range[i](u) = 0.f;
} }
} }
} }
@@ -415,14 +435,14 @@ void CLaserOdometry2D::calculateCoord()
if ((range_old[image_level](u) == 0.f) || (range_warped[image_level](u) == 0.f)) if ((range_old[image_level](u) == 0.f) || (range_warped[image_level](u) == 0.f))
{ {
range_inter[image_level](u) = 0.f; range_inter[image_level](u) = 0.f;
xx_inter[image_level](u) = 0.f; xx_inter[image_level](u) = 0.f;
yy_inter[image_level](u) = 0.f; yy_inter[image_level](u) = 0.f;
} }
else else
{ {
range_inter[image_level](u) = 0.5f*(range_old[image_level](u) + range_warped[image_level](u)); 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)); 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)); yy_inter[image_level](u) = 0.5f*(yy_old[image_level](u) + yy_warped[image_level](u));
} }
} }
} }
@@ -696,19 +716,17 @@ 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:\n" << cov_odo); ROS_INFO_STREAM_COND(verbose && false, "[rf2o] COV_ODO:\n" << cov_odo);
} }
void CLaserOdometry2D::Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan scan*/) void CLaserOdometry2D::Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan scan*/)
{ {
//Set the initial pose //Set the initial pose
laser_pose_ = ini_pose; laser_pose_ = ini_pose;
laser_oldpose_ = ini_pose; laser_oldpose_ = ini_pose;
//readLaser(scan); //readLaser(scan);
createImagePyramid(); createImagePyramid();
//readLaser(scan);
createImagePyramid();
} }
void CLaserOdometry2D::performWarping() void CLaserOdometry2D::performWarping()
@@ -786,15 +804,15 @@ void CLaserOdometry2D::performWarping()
} }
} }
void CLaserOdometry2D::filterLevelSolution() bool CLaserOdometry2D::filterLevelSolution()
{ {
// Calculate Eigenvalues and Eigenvectors // Calculate Eigenvalues and Eigenvectors
//---------------------------------------------------------- //----------------------------------------------------------
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigensolver(cov_odo); Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigensolver(cov_odo);
if (eigensolver.info() != Eigen::Success) if (eigensolver.info() != Eigen::Success)
{ {
ROS_INFO_COND(verbose, "[rf2o] ERROR: Eigensolver couldn't find a solution. Pose is not updated"); ROS_WARN_COND(verbose, "[rf2o] ERROR: Eigensolver couldn't find a solution. Pose is not updated");
return; return false;
} }
//First, we have to describe both the new linear and angular speeds in the "eigenvector" basis //First, we have to describe both the new linear and angular speeds in the "eigenvector" basis
@@ -805,6 +823,8 @@ void CLaserOdometry2D::filterLevelSolution()
kai_b = Bii.colPivHouseholderQr().solve(kai_loc_level_); 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 //Second, we have to describe both the old linear and angular speeds in the "eigenvector" basis too
//------------------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------------------
MatrixS31 kai_loc_sub; MatrixS31 kai_loc_sub;
@@ -828,52 +848,61 @@ void CLaserOdometry2D::filterLevelSolution()
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);
assert((kai_loc_sub).isApprox(Bii*kai_b_old, 1e-5) && "Ax=b has no solution." && __LINE__);
//Filter speed //Filter speed
const float cf = 15e3f*std::exp(-float(int(level))), const float cf = 15e3f*std::exp(-float(int(level))),
df = 0.05f*std::exp(-float(int(level))); df = 0.05f*std::exp(-float(int(level)));
Eigen::Matrix<float,3,1> kai_b_fil; Eigen::Matrix<float,3,1> kai_b_fil;
for (unsigned int i=0; i<3; i++) 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(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); //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 //Transform filtered speed to local reference frame and compute transformation
Eigen::Matrix<float,3,1> kai_loc_fil = Bii.inverse().colPivHouseholderQr().solve(kai_b_fil); Eigen::Matrix<float, 3, 1> 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 //transformation
const float incrx = kai_loc_fil(0)/fps; const float incrx = kai_loc_fil(0)/fps;
const float incry = kai_loc_fil(1)/fps; const float incry = kai_loc_fil(1)/fps;
const float rot = kai_loc_fil(2)/fps; const float rot = kai_loc_fil(2)/fps;
transformations[level](0,0) = std::cos(rot); transformations[level](0,0) = std::cos(rot);
transformations[level](0,1) = -std::sin(rot); transformations[level](0,1) = -std::sin(rot);
transformations[level](1,0) = std::sin(rot); transformations[level](1,0) = std::sin(rot);
transformations[level](1,1) = std::cos(rot); transformations[level](1,1) = std::cos(rot);
transformations[level](0,2) = incrx; transformations[level](0,2) = incrx;
transformations[level](1,2) = incry; transformations[level](1,2) = incry;
return true;
} }
void CLaserOdometry2D::PoseUpdate() void CLaserOdometry2D::PoseUpdate()
{ {
//First, compute the overall transformation // First, compute the overall transformation
//--------------------------------------------------- //---------------------------------------------------
Eigen::Matrix3f acu_trans; Eigen::Matrix3f acu_trans;
acu_trans.setIdentity(); acu_trans.setIdentity();
for (unsigned int i=1; i<=ctf_levels; i++) for (unsigned int i=1; i<=ctf_levels; i++)
acu_trans = transformations[i-1]*acu_trans; acu_trans = transformations[i-1]*acu_trans;
// 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*std::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());