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

rm mrpt headers

Browse Source
This commit is contained in:
Jeremie Deray
2017-08-15 17:24:24 +02:00
parent 43d0c05e0f
commit 5543259c96
2 changed files with 57 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
include/rf2o_laser_odometry/CLaserOdometry2D.h
View File

@@ -16,40 +16,20 @@
#ifndef CLaserOdometry2D_H #ifndef CLaserOdometry2D_H
#define CLaserOdometry2D_H #define CLaserOdometry2D_H
// std header
#include <iostream>
#include <fstream>
#include <numeric>
// ROS headers
#include <ros/ros.h> #include <ros/ros.h>
#include <nav_msgs/Odometry.h> #include <nav_msgs/Odometry.h>
#include <sensor_msgs/LaserScan.h> #include <sensor_msgs/LaserScan.h>
// MRPT related headers // Eigen headers
#include <mrpt/version.h>
#if MRPT_VERSION>=0x130
# include <mrpt/obs/CObservation2DRangeScan.h>
# include <mrpt/obs/CObservationOdometry.h>
typedef mrpt::obs::CObservation2DRangeScan CObservation2DRangeScan;
#else
# include <mrpt/slam/CObservation2DRangeScan.h>
# include <mrpt/slam/CObservationOdometry.h>
typedef mrpt::poses::CObservation2DRangeScan CObservation2DRangeScan;
#endif
#if MRPT_VERSION<0x150
# include <mrpt/system/threads.h>
#endif
#include <mrpt/system/os.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/utils.h>
//#include <mrpt/opengl.h>
#include <mrpt/math/CHistogram.h>
#include <boost/bind.hpp>
#include <Eigen/Dense> #include <Eigen/Dense>
#include <Eigen/Geometry> #include <Eigen/Geometry>
#include <unsupported/Eigen/MatrixFunctions> #include <unsupported/Eigen/MatrixFunctions>
#include <iostream>
#include <fstream>
#include <numeric>
namespace rf2o { namespace rf2o {
template <typename T> template <typename T>
@@ -113,7 +93,7 @@ public:
protected: protected:
bool verbose,module_initialized,first_laser_scan; bool verbose, module_initialized, first_laser_scan;
// Internal Data // Internal Data
std::vector<Eigen::MatrixXf> range; std::vector<Eigen::MatrixXf> range;
@@ -144,8 +124,6 @@ protected:
MatrixS31 Var; //3 unknowns: vx, vy, w MatrixS31 Var; //3 unknowns: vx, vy, w
IncrementCov cov_odo; IncrementCov cov_odo;
//std::string LaserVarName; //Name of the topic containing the scan lasers \laser_scan //std::string LaserVarName; //Name of the topic containing the scan lasers \laser_scan
float fps; //In Hz float fps; //In Hz
float fovh; //Horizontal FOV float fovh; //Horizontal FOV
@@ -160,7 +138,7 @@ protected:
double lin_speed, ang_speed; double lin_speed, ang_speed;
ros::Duration m_runtime; ros::WallDuration m_runtime;
ros::Time last_odom_time, current_scan_time; ros::Time last_odom_time, current_scan_time;
MatrixS31 kai_abs_; MatrixS31 kai_abs_;
@@ -193,7 +171,7 @@ protected:
void solveSystemNonLinear(); void solveSystemNonLinear();
void filterLevelSolution(); void filterLevelSolution();
void PoseUpdate(); void PoseUpdate();
void Reset(const Pose3d& ini_pose, CObservation2DRangeScan scan); void Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan scan*/);
}; };
#endif #endif // CLaserOdometry2D_H

94
src/CLaserOdometry2D.cpp
View File

@@ -69,7 +69,7 @@ void CLaserOdometry2D::init(const sensor_msgs::LaserScan& scan,
// Init module (internal) // Init module (internal)
//------------------------ //------------------------
range_wf.setSize(1, width); range_wf = Eigen::MatrixXf::Constant(1, width, 1);
//Resize vectors according to levels //Resize vectors according to levels
transformations.resize(ctf_levels); transformations.resize(ctf_levels);
@@ -94,24 +94,21 @@ void CLaserOdometry2D::init(const sensor_msgs::LaserScan& scan,
for (unsigned int i = 0; i < pyr_levels; i++) for (unsigned int i = 0; i < pyr_levels; i++)
{ {
s = pow(2.f, int(i)); s = std::pow(2.f, int(i));
cols_i = ceil(float(width) / float(s)); cols_i = std::ceil(float(width) / float(s));
range[i].resize(1, cols_i); 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); range_inter[i].resize(1, cols_i);
range_old[i].resize(1, cols_i);
range[i].assign(0.0f); xx[i] = Eigen::MatrixXf::Constant(1, cols_i, 0.f);
range_old[i].assign(0.0f); xx_old[i] = Eigen::MatrixXf::Constant(1, cols_i, 0.f);
xx[i].resize(1, cols_i);
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); 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_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) if (cols_i <= cols)
{ {
@@ -126,11 +123,10 @@ void CLaserOdometry2D::init(const sensor_msgs::LaserScan& scan,
normx.resize(1, cols); normx.resize(1, cols);
normy.resize(1, cols); normy.resize(1, cols);
norm_ang.resize(1, cols); norm_ang.resize(1, cols);
weights.setSize(1, cols); weights.resize(1, cols);
null.setSize(1, cols);
null.assign(0);
cov_odo.assign(0.f);
null = Eigen::MatrixXi::Constant(1, cols, 0);
cov_odo = IncrementCov::Zero();
fps = 1.f; //In Hz fps = 1.f; //In Hz
num_valid_range = 0; num_valid_range = 0;
@@ -175,7 +171,7 @@ bool CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan)
for (unsigned int i = 0; i<width; i++) for (unsigned int i = 0; i<width; i++)
range_wf(i) = scan.ranges[i]; range_wf(i) = scan.ranges[i];
ros::Time start = ros::Time::now(); ros::WallTime start = ros::WallTime::now();
createImagePyramid(); createImagePyramid();
@@ -224,11 +220,13 @@ bool CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan)
//8. Filter solution //8. Filter solution
filterLevelSolution(); filterLevelSolution();
} }
m_runtime = ros::Time::now() - start; m_runtime = ros::WallTime::now() - start;
ROS_INFO_COND(verbose, "[rf2o] execution time (ms): %f", (1000*m_runtime.toNSec())); ROS_INFO_COND(verbose, "[rf2o] execution time (ms): %f",
m_runtime.toSec()*double(1000));
//Update poses //Update poses
PoseUpdate(); PoseUpdate();
@@ -434,8 +432,9 @@ 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 //Compute connectivity
rtita.resize(1,cols_i); //Defined in a different way now, without inversion
rtita.assign(1.f); //Defined in a different way now, without inversion
rtita = Eigen::MatrixXf::Constant(1, cols_i, 1.f);
for (unsigned int u = 0; u < cols_i-1; u++) for (unsigned int u = 0; u < cols_i-1; u++)
{ {
@@ -489,9 +488,9 @@ void CLaserOdometry2D::calculaterangeDerivativesSurface()
void CLaserOdometry2D::computeNormals() void CLaserOdometry2D::computeNormals()
{ {
normx.assign(0.f); normx.setConstant(1, cols, 0.f);
normy.assign(0.f); normy.setConstant(1, cols, 0.f);
norm_ang.assign(0.f); norm_ang.setConstant(1, cols, 0.f);
const float incr_tita = fovh/float(cols_i-1); const float incr_tita = fovh/float(cols_i-1);
for (unsigned int u=0; u<cols_i; u++) for (unsigned int u=0; u<cols_i; u++)
@@ -518,7 +517,7 @@ void CLaserOdometry2D::computeNormals()
void CLaserOdometry2D::computeWeights() void CLaserOdometry2D::computeWeights()
{ {
//The maximum weight size is reserved at the constructor //The maximum weight size is reserved at the constructor
weights.assign(0.f); weights.setConstant(1, cols, 0.f);
//Parameters for error_linearization //Parameters for error_linearization
const float kdtita = 1.f; const float kdtita = 1.f;
@@ -543,7 +542,7 @@ void CLaserOdometry2D::computeWeights()
weights(u) = std::sqrt(1.f/w_der); weights(u) = std::sqrt(1.f/w_der);
} }
const float inv_max = 1.f/weights.maximum(); const float inv_max = 1.f / weights.maxCoeff();
weights = inv_max*weights; weights = inv_max*weights;
} }
@@ -569,8 +568,9 @@ void CLaserOdometry2D::findNullPoints()
// Solves the system without considering any robust-function // Solves the system without considering any robust-function
void CLaserOdometry2D::solveSystemOneLevel() void CLaserOdometry2D::solveSystemOneLevel()
{ {
A.resize(num_valid_range,3); A.resize(num_valid_range, 3);
B.setSize(num_valid_range,1); B.resize(num_valid_range, 1);
unsigned int cont = 0; unsigned int cont = 0;
const float kdtita = (cols_i-1)/fovh; const float kdtita = (cols_i-1)/fovh;
@@ -595,8 +595,8 @@ void CLaserOdometry2D::solveSystemOneLevel()
//Solve the linear system of equations using a minimum least squares method //Solve the linear system of equations using a minimum least squares method
Eigen::MatrixXf AtA, AtB; Eigen::MatrixXf AtA, AtB;
AtA.multiply_AtA(A); AtA = A.transpose()*A;
AtB.multiply_AtB(A,B); AtB = A.transpose()*B;
Var = AtA.ldlt().solve(AtB); Var = AtA.ldlt().solve(AtB);
//Covariance matrix calculation Cov Order -> vx,vy,wz //Covariance matrix calculation Cov Order -> vx,vy,wz
@@ -611,8 +611,8 @@ void CLaserOdometry2D::solveSystemOneLevel()
// Solves the system by considering the Cauchy M-estimator robust-function // Solves the system by considering the Cauchy M-estimator robust-function
void CLaserOdometry2D::solveSystemNonLinear() void CLaserOdometry2D::solveSystemNonLinear()
{ {
A.resize(num_valid_range,3); Aw.resize(num_valid_range,3); A.resize(num_valid_range, 3); Aw.resize(num_valid_range, 3);
B.setSize(num_valid_range,1); Bw.setSize(num_valid_range,1); B.resize(num_valid_range, 1); Bw.resize(num_valid_range, 1);
unsigned int cont = 0; unsigned int cont = 0;
const float kdtita = float(cols_i-1)/fovh; const float kdtita = float(cols_i-1)/fovh;
@@ -637,8 +637,8 @@ void CLaserOdometry2D::solveSystemNonLinear()
//Solve the linear system of equations using a minimum least squares method //Solve the linear system of equations using a minimum least squares method
Eigen::MatrixXf AtA, AtB; Eigen::MatrixXf AtA, AtB;
AtA.multiply_AtA(A); AtA = A.transpose()*A;
AtB.multiply_AtB(A,B); AtB = A.transpose()*B;
Var = AtA.ldlt().solve(AtB); Var = AtA.ldlt().solve(AtB);
//Covariance matrix calculation Cov Order -> vx,vy,wz //Covariance matrix calculation Cov Order -> vx,vy,wz
@@ -686,8 +686,8 @@ void CLaserOdometry2D::solveSystemNonLinear()
} }
//Solve the linear system of equations using a minimum least squares method //Solve the linear system of equations using a minimum least squares method
AtA.multiply_AtA(Aw); AtA = Aw.transpose()*Aw;
AtB.multiply_AtB(Aw,Bw); AtB = Aw.transpose()*Bw;
Var = AtA.ldlt().solve(AtB); Var = AtA.ldlt().solve(AtB);
res = A*Var - B; res = A*Var - B;
@@ -704,7 +704,7 @@ void CLaserOdometry2D::solveSystemNonLinear()
ROS_INFO_STREAM_COND(verbose, "[rf2o] COV_ODO:\n" << 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*/)
{ {
//Set the initial pose //Set the initial pose
laser_pose_ = ini_pose; laser_pose_ = ini_pose;
@@ -725,9 +725,9 @@ void CLaserOdometry2D::performWarping()
for (unsigned int i=1; i<=level; i++) for (unsigned int i=1; i<=level; i++)
acu_trans = transformations[i-1]*acu_trans; acu_trans = transformations[i-1]*acu_trans;
Eigen::MatrixXf wacu(1,cols_i); Eigen::MatrixXf wacu = Eigen::MatrixXf::Constant(1, cols_i, 0.f);
wacu.assign(0.f);
range_warped[image_level].assign(0.f); range_warped[image_level].setConstant(1, cols_i, 0.f);
const float cols_lim = float(cols_i-1); const float cols_lim = float(cols_i-1);
const float kdtita = cols_lim/fovh; const float kdtita = cols_lim/fovh;
@@ -898,7 +898,7 @@ void CLaserOdometry2D::PoseUpdate()
//------------------------------------------------------- //-------------------------------------------------------
laser_oldpose_ = laser_pose_; laser_oldpose_ = laser_pose_;
mrpt::math::CMatrixDouble33 aux_acu = acu_trans; // Eigen::Matrix3f 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));
@@ -909,8 +909,6 @@ void CLaserOdometry2D::PoseUpdate()
last_increment_ = pose_aux_2D; last_increment_ = pose_aux_2D;
// Compute kai_loc_old // Compute kai_loc_old
//------------------------------------------------------- //-------------------------------------------------------
phi = rf2o::getYaw(laser_pose_.rotation()); phi = rf2o::getYaw(laser_pose_.rotation());
@@ -921,7 +919,7 @@ void CLaserOdometry2D::PoseUpdate()
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),
laser_pose_.translation()(1), laser_pose_.translation()(1),
0/*TODOlaser_pose_.rotation()*/); rf2o::getYaw(laser_pose_.rotation()));
//Compose Transformations //Compose Transformations
robot_pose_ = laser_pose_ * laser_pose_on_robot_inv_; robot_pose_ = laser_pose_ * laser_pose_on_robot_inv_;
@@ -929,7 +927,7 @@ void CLaserOdometry2D::PoseUpdate()
ROS_INFO_COND(verbose, "BASEodom = [%f %f %f]", ROS_INFO_COND(verbose, "BASEodom = [%f %f %f]",
robot_pose_.translation()(0), robot_pose_.translation()(0),
robot_pose_.translation()(1), robot_pose_.translation()(1),
0/*TODOrobot_pose_.rotation()*/); rf2o::getYaw(robot_pose_.rotation()));
// Estimate linear/angular speeds (mandatory for base_local_planner) // Estimate linear/angular speeds (mandatory for base_local_planner)
// last_scan -> the last scan received // last_scan -> the last scan received