Skip to content

Commit

Permalink
improving doc
Browse files Browse the repository at this point in the history
  • Loading branch information
@JGMonroy
JGMonroy committed Apr 28, 2023
1 parent 45f8359 commit b38c68e
Show file tree
Hide file tree
Showing 2 changed files with 73 additions and 51 deletions.
11 changes: 5 additions & 6 deletions include/rf2o_laser_odometry/CLaserOdometry2D.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,12 +7,12 @@
* For more information, please refer to:
*
* Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA'16.
* Available at: http://mapir.isa.uma.es/mapirwebsite/index.php/mapir-downloads/papers/217
* Available at: http://mapir.uma.es/papersrepo/2016/2016_Jaimez_ICRA_RF2O.pdf
*
* Maintainer: Javier G. Monroy
* MAPIR group: http://mapir.isa.uma.es/
** Maintainer: Javier G. Monroy
* MAPIR group: https://mapir.isa.uma.es
*
* Modifications: Jeremie Deray
* Modifications: Jeremie Deray & (see contributons on github)
******************************************************************************************** */

#ifndef CLaserOdometry2D_H
Expand All @@ -22,7 +22,6 @@
#include <iostream>
#include <fstream>
#include <numeric>

// ROS headers
#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/odometry.hpp>
Expand Down Expand Up @@ -65,12 +64,12 @@ inline Eigen::Matrix<T, 3, 3> matrixYaw(const T yaw)
}

using Scalar = float;

using Pose2d = Eigen::Isometry2d;
using Pose3d = Eigen::Isometry3d;
using MatrixS31 = Eigen::Matrix<Scalar, 3, 1>;
using IncrementCov = Eigen::Matrix<Scalar, 3, 3>;


class CLaserOdometry2D: public rclcpp::Node
{
public:
Expand Down
113 changes: 68 additions & 45 deletions src/CLaserOdometry2D.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,22 +7,22 @@
* For more information, please refer to:
*
* Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA'16.
* Available at: http://mapir.isa.uma.es/mapirwebsite/index.php/mapir-downloads/papers/217
* Available at: http://mapir.uma.es/papersrepo/2016/2016_Jaimez_ICRA_RF2O.pdf
*
* Maintainer: Javier G. Monroy
* MAPIR group: http://mapir.isa.uma.es/
* MAPIR group: https://mapir.isa.uma.es
*
* Modifications: Jeremie Deray
* Modifications: Jeremie Deray & (see contributons on github)
******************************************************************************************** */

#include "rf2o_laser_odometry/CLaserOdometry2D.hpp"

namespace rf2o {

// --------------------------------------------
// CLaserOdometry2D
//---------------------------------------------

/**
* Constructor that inherits from Node
*/
CLaserOdometry2D::CLaserOdometry2D() :
Node("CLaserOdometry2D"),
verbose(false),
Expand Down Expand Up @@ -164,7 +164,7 @@ void CLaserOdometry2D::init(const sensor_msgs::msg::LaserScan& scan,
kai_loc_old_ = MatrixS31::Zero();

module_initialized = true;
last_odom_time = scan.header.stamp;
last_odom_time = scan.header.stamp; // the time of this first scan
}


Expand All @@ -173,71 +173,84 @@ const Pose3d& CLaserOdometry2D::getIncrement() const
return last_increment_;
}


const Eigen::Matrix<float, 3, 3>& CLaserOdometry2D::getIncrementCovariance() const
{
return cov_odo;
}


Pose3d& CLaserOdometry2D::getPose()
{
return robot_pose_;
}


const Pose3d& CLaserOdometry2D::getPose() const
{
return robot_pose_;
}


/**
* Performs multilevel odometry calculation from a pair of scan lasers (previous-current)
* Odometry estimation in ROS is cumulative, that is, it adds incrementally to update the robot "odom"
* @param scan the last laser scan received (will be compared with the previous one stored)
*/
bool CLaserOdometry2D::odometryCalculation(const sensor_msgs::msg::LaserScan& scan)
{
//==================================================================================
// DIFERENTIAL ODOMETRY MULTILEVEL
//==================================================================================
//=====================================
// DIFERENTIAL ODOMETRY MULTILEVEL
//=====================================

//copy laser scan to internal variable
// copy @param scan to internal variable (we already did it for the previous scan)
range_wf = Eigen::Map<const Eigen::MatrixXf>(scan.ranges.data(), width, 1);

// Keep record of times
auto start = get_clock()->now();

// Create pyramid from current scan
createImagePyramid();

//Coarse-to-fine scheme
// Coarse-to-fine scheme (pyramid lvls)
for (unsigned int i=0; i<ctf_levels; i++)
{
//Previous computations
// Clear previous computations
transformations[i].setIdentity();

// Keep record of current level (for other methods)
level = i;
unsigned int s = std::pow(2.f,int(ctf_levels-(i+1)));
cols_i = std::ceil(float(cols)/float(s));
image_level = ctf_levels - i + std::round(std::log2(std::round(float(width)/float(cols)))) - 1;

//1. Perform warping
// 1. Perform warping
if (i == 0)
{
// No need for the first lvl
range_warped[image_level] = range[image_level];
xx_warped[image_level] = xx[image_level];
yy_warped[image_level] = yy[image_level];
}
else
performWarping();

//2. Calculate inter coords
// 2. Calculate inter coords
calculateCoord();

//3. Find null points
// 3. Find null points
findNullPoints();

//4. Compute derivatives
// 4. Compute derivatives
calculaterangeDerivativesSurface();

//5. Compute normals
// 5. Compute normals
//computeNormals();

//6. Compute weights
// 6. Compute weights
computeWeights();

//7. Solve odometry
// 7. Solve odometry
if (num_valid_range > 3)
{
solveSystemNonLinear();
Expand All @@ -254,51 +267,53 @@ bool CLaserOdometry2D::odometryCalculation(const sensor_msgs::msg::LaserScan& sc
continue;
}

//8. Filter solution
// 8. Filter solution
if (!filterLevelSolution()) return false;
}
} // end pyramid lvls

// Get computation time
auto m_runtime = get_clock()->now() - start;

RCLCPP_INFO(get_logger(), "[rf2o] execution time (ms): %f",
RCLCPP_INFO(get_logger(), "execution time (ms): %f",
m_runtime.seconds()*double(1000));

//Update poses
// Update poses with the new odom
PoseUpdate();

return true;
}


/**
* Creates the Pyramid (multi-resolution) from the current laser range data
*/
void CLaserOdometry2D::createImagePyramid()
{
const float max_range_dif = 0.3f;

//Push the frames back
// Push the scans back
range_old.swap(range);
xx_old.swap(xx);
yy_old.swap(yy);

//The number of levels of the pyramid does not match the number of levels used
//in the odometry computation (because we sometimes want to finish with lower resolutions)

// The number of levels of the pyramid does not always match the number of levels used
// in the odometry computation (because we sometimes want to finish with lower resolutions)
unsigned int pyr_levels = std::round(std::log2(std::round(float(width)/float(cols)))) + ctf_levels;

//Generate levels
// Generate Pyramid levels
const float max_range_dif = 0.3f;
for (unsigned int i = 0; i<pyr_levels; i++)
{
unsigned int s = std::pow(2.f,int(i));
cols_i = std::ceil(float(width)/float(s));

const unsigned int i_1 = i-1;

//First level -> Filter (not downsampling);
// First level -> Filter (not downsampling);
if (i == 0)
{
for (unsigned int u = 0; u < cols_i; u++)
{
const float dcenter = range_wf(u);

//Inner pixels
// Inner pixels (avoid first and last points in the scan)
if ((u>1)&&(u<cols_i-2))
{
if (std::isfinite(dcenter) && dcenter > 0.f)
Expand All @@ -322,7 +337,7 @@ void CLaserOdometry2D::createImagePyramid()
range[i](u) = 0.f;
}

//Boundary
// Boundary points
else
{
if (std::isfinite(dcenter) && dcenter > 0.f)
Expand Down Expand Up @@ -352,16 +367,15 @@ void CLaserOdometry2D::createImagePyramid()
}
}

// Downsampling
//-----------------------------------------------------------------------------
// Second level and forth -> Downsampling
else
{
for (unsigned int u = 0; u < cols_i; u++)
{
const int u2 = 2*u;
const float dcenter = range[i_1](u2);

//Inner pixels
// Inner pixels (avoid first/last point in the scan)
if ((u>0)&&(u<cols_i-1))
{
if (dcenter > 0.f)
Expand Down Expand Up @@ -419,7 +433,7 @@ void CLaserOdometry2D::createImagePyramid()
}
}

//Calculate coordinates "xy" of the points
// Calculate coordinates "xy" of the points
for (unsigned int u = 0; u < cols_i; u++)
{
if (range[i](u) > 0.f)
Expand All @@ -437,6 +451,7 @@ void CLaserOdometry2D::createImagePyramid()
}
}


void CLaserOdometry2D::calculateCoord()
{
for (unsigned int u = 0; u < cols_i; u++)
Expand All @@ -456,6 +471,7 @@ void CLaserOdometry2D::calculateCoord()
}
}


void CLaserOdometry2D::calculaterangeDerivativesSurface()
{
//The gradient size ir reserved at the maximum size (at the constructor)
Expand Down Expand Up @@ -738,6 +754,9 @@ void CLaserOdometry2D::Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan s
createImagePyramid();
}

/**
* Performs warping on the scan points of the current pyramid lvl(coarse2fine)
*/
void CLaserOdometry2D::performWarping()
{
Eigen::Matrix3f acu_trans;
Expand Down Expand Up @@ -820,7 +839,7 @@ bool CLaserOdometry2D::filterLevelSolution()
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigensolver(cov_odo);
if (eigensolver.info() != Eigen::Success)
{
RCLCPP_WARN(get_logger(), "[rf2o] ERROR: Eigensolver couldn't find a solution. Pose is not updated");
RCLCPP_WARN(get_logger(), "WARNING: Eigensolver couldn't find a solution. Pose is not updated");
return false;
}

Expand Down Expand Up @@ -890,10 +909,14 @@ bool CLaserOdometry2D::filterLevelSolution()
return true;
}


/**
* Updates the laser and robot poses after analyzing the last scan
* To do so, we need to analyze the coarse2fine pyramid
*/
void CLaserOdometry2D::PoseUpdate()
{
// First, compute the overall transformation
//---------------------------------------------------
// First, compute the overall transformation
Eigen::Matrix3f acu_trans;
acu_trans.setIdentity();

Expand Down Expand Up @@ -943,15 +966,15 @@ void CLaserOdometry2D::PoseUpdate()
kai_loc_old_(1) = -kai_abs_(0)*std::sin(phi) + kai_abs_(1)*std::cos(phi);
kai_loc_old_(2) = kai_abs_(2);

RCLCPP_INFO(get_logger(), "[rf2o] LASERodom = [%f %f %f]",
RCLCPP_INFO(get_logger(), "Laser odom [x,y,yaw]=[%f %f %f]",
laser_pose_.translation()(0),
laser_pose_.translation()(1),
rf2o::getYaw(laser_pose_.rotation()));

//Compose Transformations
// Compose Transformations (robot odom)
robot_pose_ = laser_pose_ * laser_pose_on_robot_inv_;

RCLCPP_INFO(get_logger(), "BASEodom = [%f %f %f]",
RCLCPP_INFO(get_logger(), "Robot-base odom [x,y,yaw]=[%f %f %f]",
robot_pose_.translation()(0),
robot_pose_.translation()(1),
rf2o::getYaw(robot_pose_.rotation()));
Expand Down

0 comments on commit b38c68e

Please sign in to comment.