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 {
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>
inline typename Eigen::MatrixBase<Derived>::Scalar
@@ -170,7 +170,7 @@ protected:
void findNullPoints();
void solveSystemOneLevel();
void solveSystemNonLinear();
void filterLevelSolution();
bool filterLevelSolution();
void PoseUpdate();
void Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan scan*/);
};

83
src/CLaserOdometry2D.cpp
View File

@@ -24,8 +24,16 @@ namespace rf2o {
//---------------------------------------------
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())
{
//
}
@@ -51,24 +59,26 @@ void CLaserOdometry2D::init(const sensor_msgs::LaserScan& scan,
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
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
Pose3d robotInitialPose = Pose3d::Identity();
const geometry_msgs::Pose _src = initial_robot_pose;
Pose3d robot_initial_pose = Pose3d::Identity();
robotInitialPose = Eigen::Quaterniond(_src.orientation.w,
_src.orientation.x,
_src.orientation.y,
_src.orientation.z);
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);
robotInitialPose.translation()(0) = _src.position.x;
robotInitialPose.translation()(1) = _src.position.y;
robot_initial_pose.translation()(0) = initial_robot_pose.position.x;
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
laser_pose_ = robotInitialPose * laser_pose_on_robot_;
laser_pose_ = robot_initial_pose * laser_pose_on_robot_;
laser_oldpose_ = laser_oldpose_;
// Init module (internal)
@@ -136,7 +146,11 @@ void CLaserOdometry2D::init(const sensor_msgs::LaserScan& scan,
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];
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_loc_old_ = MatrixS31::Zero();
@@ -172,8 +186,7 @@ bool CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan)
//==================================================================================
//copy laser scan to internal variable
for (unsigned int i = 0; i<width; i++)
range_wf(i) = scan.ranges[i];
range_wf = Eigen::Map<const Eigen::MatrixXf>(scan.ranges.data(), width, 1);
ros::WallTime start = ros::WallTime::now();
@@ -221,10 +234,19 @@ bool CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan)
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;
}
//8. Filter solution
filterLevelSolution();
if (!filterLevelSolution()) return false;
}
m_runtime = ros::WallTime::now() - start;
@@ -289,7 +311,6 @@ void CLaserOdometry2D::createImagePyramid()
}
else
range[i](u) = 0.f;
}
//Boundary
@@ -318,7 +339,6 @@ void CLaserOdometry2D::createImagePyramid()
}
else
range[i](u) = 0.f;
}
}
}
@@ -696,7 +716,7 @@ void CLaserOdometry2D::solveSystemNonLinear()
cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm();
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*/)
@@ -707,8 +727,6 @@ void CLaserOdometry2D::Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan s
//readLaser(scan);
createImagePyramid();
//readLaser(scan);
createImagePyramid();
}
void CLaserOdometry2D::performWarping()
@@ -786,15 +804,15 @@ void CLaserOdometry2D::performWarping()
}
}
void CLaserOdometry2D::filterLevelSolution()
bool CLaserOdometry2D::filterLevelSolution()
{
// Calculate Eigenvalues and Eigenvectors
//----------------------------------------------------------
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> 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;
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
@@ -805,6 +823,8 @@ void CLaserOdometry2D::filterLevelSolution()
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;
@@ -828,6 +848,8 @@ void CLaserOdometry2D::filterLevelSolution()
Eigen::Matrix<float,3,1> 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)));
@@ -835,23 +857,28 @@ void CLaserOdometry2D::filterLevelSolution()
Eigen::Matrix<float,3,1> 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(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<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
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()
@@ -860,20 +887,22 @@ void CLaserOdometry2D::PoseUpdate()
//---------------------------------------------------
Eigen::Matrix3f 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());