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

fixes after rebasing

Browse Source
This commit is contained in:
Jeremie Deray
2017-08-12 18:11:54 +02:00
parent 92b817eb17
commit 576817a8b6
2 changed files with 22 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
include/rf2o_laser_odometry/CLaserOdometry2D.h
View File

@@ -40,7 +40,7 @@
#include <mrpt/system/os.h> #include <mrpt/system/os.h>
#include <mrpt/poses/CPose3D.h> #include <mrpt/poses/CPose3D.h>
#include <mrpt/utils.h> #include <mrpt/utils.h>
#include <mrpt/opengl.h> //#include <mrpt/opengl.h>
#include <mrpt/math/CHistogram.h> #include <mrpt/math/CHistogram.h>
#include <boost/bind.hpp> #include <boost/bind.hpp>
@@ -63,17 +63,7 @@ public:
bool is_initialized(); bool is_initialized();
<<<<<<< HEAD
protected:
ros::NodeHandle n;
sensor_msgs::LaserScan last_scan;
bool module_initialized,first_laser_scan,new_scan_available, GT_pose_initialized, verbose;
tf::TransformListener tf_listener; //Do not put inside the callback
tf::TransformBroadcaster odom_broadcaster;
nav_msgs::Odometry initial_robot_pose;
=======
void odometryCalculation(const sensor_msgs::LaserScan& scan); void odometryCalculation(const sensor_msgs::LaserScan& scan);
>>>>>>> wip separating node from computation
void setLaserPose(const mrpt::poses::CPose3D& laser_pose); void setLaserPose(const mrpt::poses::CPose3D& laser_pose);
@@ -85,7 +75,7 @@ protected:
protected: protected:
bool 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;
@@ -101,7 +91,7 @@ protected:
std::vector<Eigen::MatrixXf> yy_old; std::vector<Eigen::MatrixXf> yy_old;
std::vector<Eigen::MatrixXf> yy_warped; std::vector<Eigen::MatrixXf> yy_warped;
std::vector<Eigen::MatrixXf> transformations; std::vector<Eigen::MatrixXf> transformations;
Eigen::MatrixXf range_wf; Eigen::MatrixXf range_wf;
Eigen::MatrixXf dtita; Eigen::MatrixXf dtita;
Eigen::MatrixXf dt; Eigen::MatrixXf dt;
@@ -160,7 +150,7 @@ protected:
void calculateCoord(); void calculateCoord();
void performWarping(); void performWarping();
void calculaterangeDerivativesSurface(); void calculaterangeDerivativesSurface();
void computeNormals(); void computeNormals();
void computeWeights(); void computeWeights();
void findNullPoints(); void findNullPoints();
void solveSystemOneLevel(); void solveSystemOneLevel();

41
src/CLaserOdometry2D.cpp
View File

@@ -50,9 +50,9 @@ void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan,
const geometry_msgs::Pose& initial_robot_pose) const geometry_msgs::Pose& initial_robot_pose)
{ {
//Got an initial scan laser, obtain its parametes //Got an initial scan laser, obtain its parametes
if (verbose) ROS_INFO_COND(verbose, "[rf2o] Got first Laser Scan .... Configuring node");
ROS_INFO("[rf2o] Got first Laser Scan .... Configuring node");
width = last_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 reolution. Should be similar to the width parameter
fovh = fabs(scan.angle_max - scan.angle_min); // Horizontal Laser's FOV fovh = fabs(scan.angle_max - scan.angle_min); // Horizontal Laser's FOV
@@ -77,8 +77,8 @@ void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan,
//robotInitialPose.phi(yaw); //robotInitialPose.phi(yaw);
//Set the initial pose //Set the initial pose
laser_pose = robotInitialPose + LaserPoseOnTheRobot; laser_pose = robotInitialPose + laser_pose_on_robot;
laser_oldpose = robotInitialPose + LaserPoseOnTheRobot; laser_oldpose = robotInitialPose + laser_pose_on_robot;
// Init module (internal) // Init module (internal)
//------------------------ //------------------------
@@ -240,8 +240,8 @@ void CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan)
} }
m_runtime = 1000*m_clock.Tac(); m_runtime = 1000*m_clock.Tac();
if (verbose)
ROS_INFO("[rf2o] execution time (ms): %f", m_runtime); ROS_INFO_COND(verbose, "[rf2o] execution time (ms): %f", m_runtime);
//Update poses //Update poses
PoseUpdate(); PoseUpdate();
@@ -704,8 +704,7 @@ 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;
if (verbose) ROS_INFO_STREAM_COND(verbose, "[rf2o] COV_ODO: " << cov_odo);
std::cout << endl << "[rf2o] COV_ODO: " << cov_odo << endl;
} }
void CLaserOdometry2D::Reset(mrpt::poses::CPose3D ini_pose, CObservation2DRangeScan scan) void CLaserOdometry2D::Reset(mrpt::poses::CPose3D ini_pose, CObservation2DRangeScan scan)
@@ -723,7 +722,7 @@ void CLaserOdometry2D::Reset(mrpt::poses::CPose3D ini_pose, CObservation2DRangeS
void CLaserOdometry2D::performWarping() void CLaserOdometry2D::performWarping()
{ {
Matrix3f acu_trans; Eigen::Matrix3f acu_trans;
acu_trans.setIdentity(); acu_trans.setIdentity();
for (unsigned int i=1; i<=level; i++) for (unsigned int i=1; i<=level; i++)
@@ -804,11 +803,10 @@ void CLaserOdometry2D::filterLevelSolution()
{ {
// Calculate Eigenvalues and Eigenvectors // Calculate Eigenvalues and Eigenvectors
//---------------------------------------------------------- //----------------------------------------------------------
SelfAdjointEigenSolver<MatrixXf> eigensolver(cov_odo); Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigensolver(cov_odo);
if (eigensolver.info() != Success) if (eigensolver.info() != Eigen::Success)
{ {
if (verbose) ROS_INFO_COND(verbose, "[rf2o] ERROR: Eigensolver couldn't find a solution. Pose is not updated");
printf("[rf2o] ERROR: Eigensolver couldn't find a solution. Pose is not updated");
return; return;
} }
@@ -910,9 +908,9 @@ void CLaserOdometry2D::PoseUpdate()
//------------------------------------------------------- //-------------------------------------------------------
laser_oldpose = laser_pose; laser_oldpose = laser_pose;
math::CMatrixDouble33 aux_acu = acu_trans; mrpt::math::CMatrixDouble33 aux_acu = acu_trans;
poses::CPose2D pose_aux_2D(acu_trans(0,2), acu_trans(1,2), kai_loc(2)/fps); mrpt::poses::CPose2D pose_aux_2D(acu_trans(0,2), acu_trans(1,2), kai_loc(2)/fps);
laser_pose = laser_pose + CPose3D(pose_aux_2D); laser_pose = laser_pose + mrpt::poses::CPose3D(pose_aux_2D);
last_increment = pose_aux_2D; last_increment = pose_aux_2D;
@@ -925,14 +923,11 @@ void CLaserOdometry2D::PoseUpdate()
kai_loc_old(1) = -kai_abs(0)*sin(phi) + kai_abs(1)*cos(phi); kai_loc_old(1) = -kai_abs(0)*sin(phi) + kai_abs(1)*cos(phi);
kai_loc_old(2) = kai_abs(2); kai_loc_old(2) = kai_abs(2);
if (verbose) ROS_INFO_COND(verbose, "[rf2o] LASERodom = [%f %f %f]",laser_pose.x(),laser_pose.y(),laser_pose.yaw());
ROS_INFO("[rf2o] LASERodom = [%f %f %f]",laser_pose.x(),laser_pose.y(),laser_pose.yaw());
//Compose Transformations //Compose Transformations
robot_pose = laser_pose + LaserPoseOnTheRobot_inv; robot_pose = laser_pose + laser_pose_on_robot_inv;
if (verbose) ROS_INFO_COND(verbose, "BASEodom = [%f %f %f]",robot_pose.x(),robot_pose.y(),robot_pose.yaw());
ROS_INFO("BASEodom = [%f %f %f]",robot_pose.x(),robot_pose.y(),robot_pose.yaw());
// 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