mirror of
https://github.com/MAPIRlab/rf2o_laser_odometry.git
synced 2023-04-06 19:28:55 +08:00
Merge pull request #10 from artivis/mrpt_free
[WIP] Remove MRPT dependency
This commit is contained in:
Javier G. Monroy
GitHub
@@ -19,22 +19,12 @@ find_package(catkin REQUIRED COMPONENTS
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
tf
|
||||
cmake_modules
|
||||
)
|
||||
|
||||
set(MRPT_DONT_USE_DBG_LIBS 1)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
find_package(Boost REQUIRED COMPONENTS system)
|
||||
find_package(cmake_modules REQUIRED)
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(MRPT REQUIRED base obs) # maps slam
|
||||
|
||||
#include_directories(${MRPT_INCLUDE_DIRS})
|
||||
MESSAGE( STATUS "MRPT_INCLUDE_DIRS: " ${MRPT_INCLUDE_DIRS})
|
||||
#link_directories(${MRPT_LIBRARY_DIRS})
|
||||
MESSAGE( STATUS "MRPT_LIBRARY_DIRS: " ${MRPT_LIBRARIES})
|
||||
|
||||
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
@@ -46,10 +36,10 @@ MESSAGE( STATUS "MRPT_LIBRARY_DIRS: " ${MRPT_LIBRARIES})
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS}
|
||||
LIBRARIES ${PROJECT_NAME}
|
||||
CATKIN_DEPENDS nav_msgs roscpp sensor_msgs std_msgs tf
|
||||
DEPENDS MRPT
|
||||
DEPENDS #Eigen3
|
||||
)
|
||||
|
||||
## Specify additional locations of header files
|
||||
@@ -60,15 +50,12 @@ include_directories(
|
||||
SYSTEM
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${Boost_INCLUDE_DIRS}
|
||||
${EIGEN_INCLUDE_DIRS}
|
||||
${MRPT_INCLUDE_DIRS}
|
||||
${EIGEN3_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Declare a cpp library
|
||||
add_library(${PROJECT_NAME}
|
||||
src/CLaserOdometry2D.cpp
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${MRPT_LIBS})
|
||||
add_library(${PROJECT_NAME} src/CLaserOdometry2D.cpp)
|
||||
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||
|
||||
## Declare a cpp executable
|
||||
add_executable(rf2o_laser_odometry_node src/CLaserOdometry2DNode.cpp)
|
||||
|
||||
@@ -6,8 +6,3 @@ RF2O is a fast and precise method to estimate the planar motion of a lidar from
|
||||
Its very low computational cost (0.9 milliseconds on a single CPU core) together whit its high precission, makes RF2O a suitable method for those robotic applications that require planar odometry.
|
||||
|
||||
For full description of the algorithm, please refer to: **Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA 2016** Available at: http://mapir.isa.uma.es/work/rf2o
|
||||
|
||||
|
||||
# Requirements
|
||||
RF2O core is implemented within the **Mobile Robot Programming Toolkit** [MRPT](http://www.mrpt.org/), so it is necessary to install this powerful library (see instructions [here](http://www.mrpt.org/download-mrpt/))
|
||||
So far RF2O has been tested with the Ubuntu official repository version (MRPT v1.3), and we are working to update it to MRPT v.1.9
|
||||
|
||||
@@ -11,67 +11,87 @@
|
||||
*
|
||||
* Maintainer: Javier G. Monroy
|
||||
* MAPIR group: http://mapir.isa.uma.es/
|
||||
*
|
||||
* Modifications: Jeremie Deray
|
||||
******************************************************************************************** */
|
||||
|
||||
#ifndef CLaserOdometry2D_H
|
||||
#define CLaserOdometry2D_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
|
||||
// MRPT related 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 <unsupported/Eigen/MatrixFunctions>
|
||||
// std header
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <numeric>
|
||||
|
||||
// ROS headers
|
||||
#include <ros/ros.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
|
||||
// Eigen headers
|
||||
#include <Eigen/Dense>
|
||||
#include <Eigen/Geometry>
|
||||
#include <unsupported/Eigen/MatrixFunctions>
|
||||
|
||||
namespace rf2o {
|
||||
|
||||
template <typename T>
|
||||
inline T sign(const T x) { return x<T(0) ? -1:1; }
|
||||
|
||||
template <typename Derived>
|
||||
inline typename Eigen::MatrixBase<Derived>::Scalar
|
||||
getYaw(const Eigen::MatrixBase<Derived>& r)
|
||||
{
|
||||
return std::atan2( r(1, 0), r(0, 0) );
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline Eigen::Matrix<T, 3, 3> matrixRollPitchYaw(const T roll,
|
||||
const T pitch,
|
||||
const T yaw)
|
||||
{
|
||||
const Eigen::AngleAxis<T> ax = Eigen::AngleAxis<T>(roll, Eigen::Matrix<T, 3, 1>::UnitX());
|
||||
const Eigen::AngleAxis<T> ay = Eigen::AngleAxis<T>(pitch, Eigen::Matrix<T, 3, 1>::UnitY());
|
||||
const Eigen::AngleAxis<T> az = Eigen::AngleAxis<T>(yaw, Eigen::Matrix<T, 3, 1>::UnitZ());
|
||||
|
||||
return (az * ay * ax).toRotationMatrix().matrix();
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline Eigen::Matrix<T, 3, 3> matrixYaw(const T yaw)
|
||||
{
|
||||
return matrixRollPitchYaw<T>(0, 0, yaw);
|
||||
}
|
||||
|
||||
class CLaserOdometry2D
|
||||
{
|
||||
public:
|
||||
CLaserOdometry2D();
|
||||
~CLaserOdometry2D();
|
||||
|
||||
void Init(const sensor_msgs::LaserScan& scan,
|
||||
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>;
|
||||
|
||||
CLaserOdometry2D();
|
||||
virtual ~CLaserOdometry2D() = default;
|
||||
|
||||
void init(const sensor_msgs::LaserScan& scan,
|
||||
const geometry_msgs::Pose& initial_robot_pose);
|
||||
|
||||
bool is_initialized();
|
||||
|
||||
void odometryCalculation(const sensor_msgs::LaserScan& scan);
|
||||
bool odometryCalculation(const sensor_msgs::LaserScan& scan);
|
||||
|
||||
void setLaserPose(const mrpt::poses::CPose3D& laser_pose);
|
||||
void setLaserPose(const Pose3d& laser_pose);
|
||||
|
||||
const mrpt::poses::CPose3D& getIncrement() const;
|
||||
const Pose3d& getIncrement() const;
|
||||
|
||||
const Eigen::Matrix<float, 3, 3>& getIncrementCovariance() const;
|
||||
const IncrementCov& getIncrementCovariance() const;
|
||||
|
||||
mrpt::poses::CPose3D& getPose();
|
||||
const mrpt::poses::CPose3D& getPose() const;
|
||||
Pose3d& getPose();
|
||||
const Pose3d& getPose() const;
|
||||
|
||||
protected:
|
||||
|
||||
@@ -102,10 +122,9 @@ protected:
|
||||
|
||||
Eigen::MatrixXf A,Aw;
|
||||
Eigen::MatrixXf B,Bw;
|
||||
Eigen::Matrix<float, 3, 1> Var; //3 unknowns: vx, vy, w
|
||||
Eigen::Matrix<float, 3, 3> cov_odo;
|
||||
|
||||
|
||||
MatrixS31 Var; //3 unknowns: vx, vy, w
|
||||
IncrementCov cov_odo;
|
||||
|
||||
//std::string LaserVarName; //Name of the topic containing the scan lasers \laser_scan
|
||||
float fps; //In Hz
|
||||
@@ -121,30 +140,26 @@ protected:
|
||||
|
||||
double lin_speed, ang_speed;
|
||||
|
||||
//mrpt::gui::CDisplayWindowPlots window;
|
||||
mrpt::utils::CTicTac m_clock;
|
||||
float m_runtime;
|
||||
ros::WallDuration m_runtime;
|
||||
ros::Time last_odom_time, current_scan_time;
|
||||
|
||||
mrpt::math::CMatrixFloat31 kai_abs;
|
||||
mrpt::math::CMatrixFloat31 kai_loc;
|
||||
mrpt::math::CMatrixFloat31 kai_loc_old;
|
||||
mrpt::math::CMatrixFloat31 kai_loc_level;
|
||||
MatrixS31 kai_abs_;
|
||||
MatrixS31 kai_loc_;
|
||||
MatrixS31 kai_loc_old_;
|
||||
MatrixS31 kai_loc_level_;
|
||||
|
||||
mrpt::poses::CPose3D last_increment;
|
||||
Pose3d last_increment_;
|
||||
Pose3d laser_pose_on_robot_;
|
||||
Pose3d laser_pose_on_robot_inv_;
|
||||
Pose3d laser_pose_;
|
||||
Pose3d laser_oldpose_;
|
||||
Pose3d robot_pose_;
|
||||
Pose3d robot_oldpose_;
|
||||
|
||||
mrpt::poses::CPose3D laser_pose_on_robot;
|
||||
mrpt::poses::CPose3D laser_pose_on_robot_inv;
|
||||
|
||||
mrpt::poses::CPose3D laser_pose;
|
||||
mrpt::poses::CPose3D laser_oldpose;
|
||||
mrpt::poses::CPose3D robot_pose;
|
||||
mrpt::poses::CPose3D robot_oldpose;
|
||||
bool test;
|
||||
std::vector<double> last_m_lin_speeds;
|
||||
std::vector<double> last_m_ang_speeds;
|
||||
|
||||
|
||||
// Methods
|
||||
void createImagePyramid();
|
||||
void calculateCoord();
|
||||
@@ -155,9 +170,11 @@ protected:
|
||||
void findNullPoints();
|
||||
void solveSystemOneLevel();
|
||||
void solveSystemNonLinear();
|
||||
void filterLevelSolution();
|
||||
bool filterLevelSolution();
|
||||
void PoseUpdate();
|
||||
void Reset(mrpt::poses::CPose3D ini_pose, CObservation2DRangeScan scan);
|
||||
void Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan scan*/);
|
||||
};
|
||||
|
||||
#endif
|
||||
} /* namespace rf2o */
|
||||
|
||||
#endif // CLaserOdometry2D_H
|
||||
|
||||
@@ -24,8 +24,7 @@
|
||||
<build_depend>tf</build_depend>
|
||||
<build_depend>cmake_modules</build_depend> <!-- A common repository for CMake Modules which are not distributed with CMake but are commonly used by ROS packages. -->
|
||||
<!-- https://github.com/ros/cmake_modules/blob/0.3-devel/README.md -->
|
||||
<build_depend>mrpt</build_depend> <!-- Depend on mrpt system pkgs: http://www.mrpt.org/ -->
|
||||
|
||||
<build_depend>eigen</build_depend>
|
||||
|
||||
<run_depend>nav_msgs</run_depend>
|
||||
<run_depend>roscpp</run_depend>
|
||||
@@ -33,6 +32,6 @@
|
||||
<run_depend>std_msgs</run_depend>
|
||||
<run_depend>tf</run_depend>
|
||||
<run_depend>cmake_modules</run_depend> <!-- For aditional dependencies such as Eigen -->
|
||||
<run_depend>mrpt</run_depend> <!-- Depend on mrpt system pkgs -->
|
||||
<run_depend>eigen</run_depend>
|
||||
|
||||
</package>
|
||||
|
||||
@@ -11,34 +11,39 @@
|
||||
*
|
||||
* Maintainer: Javier G. Monroy
|
||||
* MAPIR group: http://mapir.isa.uma.es/
|
||||
*
|
||||
* Modifications: Jeremie Deray
|
||||
******************************************************************************************** */
|
||||
|
||||
#include "rf2o_laser_odometry/CLaserOdometry2D.h"
|
||||
|
||||
namespace rf2o {
|
||||
|
||||
// --------------------------------------------
|
||||
// CLaserOdometry2D
|
||||
//---------------------------------------------
|
||||
|
||||
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())
|
||||
{
|
||||
//
|
||||
}
|
||||
|
||||
|
||||
CLaserOdometry2D::~CLaserOdometry2D()
|
||||
{
|
||||
//
|
||||
}
|
||||
|
||||
void CLaserOdometry2D::setLaserPose(const mrpt::poses::CPose3D& laser_pose)
|
||||
void CLaserOdometry2D::setLaserPose(const Pose3d& laser_pose)
|
||||
{
|
||||
//Set laser pose on the robot
|
||||
|
||||
laser_pose_on_robot = laser_pose;
|
||||
laser_pose_on_robot_inv = laser_pose_on_robot;
|
||||
laser_pose_on_robot_inv.inverse();
|
||||
laser_pose_on_robot_ = laser_pose;
|
||||
laser_pose_on_robot_inv_ = laser_pose_on_robot_.inverse();
|
||||
}
|
||||
|
||||
bool CLaserOdometry2D::is_initialized()
|
||||
@@ -46,7 +51,7 @@ bool CLaserOdometry2D::is_initialized()
|
||||
return module_initialized;
|
||||
}
|
||||
|
||||
void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan,
|
||||
void CLaserOdometry2D::init(const sensor_msgs::LaserScan& scan,
|
||||
const geometry_msgs::Pose& initial_robot_pose)
|
||||
{
|
||||
//Got an initial scan laser, obtain its parametes
|
||||
@@ -54,35 +59,31 @@ 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
|
||||
|
||||
//Robot initial pose (see MQTT:bridge)
|
||||
mrpt::poses::CPose3D robotInitialPose;
|
||||
geometry_msgs::Pose _src = initial_robot_pose;
|
||||
Pose3d robot_initial_pose = Pose3d::Identity();
|
||||
|
||||
robotInitialPose.x(_src.position.x);
|
||||
robotInitialPose.y(_src.position.y);
|
||||
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);
|
||||
|
||||
mrpt::math::CQuaternionDouble quat;
|
||||
quat.x(_src.orientation.x);
|
||||
quat.y(_src.orientation.y);
|
||||
quat.z(_src.orientation.z);
|
||||
quat.r(_src.orientation.w);
|
||||
double roll, pitch, yaw;
|
||||
quat.rpy(roll, pitch, yaw);
|
||||
robotInitialPose.setYawPitchRoll(yaw,pitch,roll);
|
||||
//robotInitialPose.phi(yaw);
|
||||
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_oldpose = robotInitialPose + laser_pose_on_robot;
|
||||
laser_pose_ = robot_initial_pose * laser_pose_on_robot_;
|
||||
laser_oldpose_ = laser_oldpose_;
|
||||
|
||||
// Init module (internal)
|
||||
//------------------------
|
||||
range_wf.setSize(1, width);
|
||||
range_wf = Eigen::MatrixXf::Constant(1, width, 1);
|
||||
|
||||
//Resize vectors according to levels
|
||||
transformations.resize(ctf_levels);
|
||||
@@ -91,7 +92,7 @@ void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan,
|
||||
|
||||
//Resize pyramid
|
||||
unsigned int s, cols_i;
|
||||
const unsigned int pyr_levels = round(log2(round(float(width) / float(cols)))) + ctf_levels;
|
||||
const unsigned int pyr_levels = std::round(std::log2(round(float(width) / float(cols)))) + ctf_levels;
|
||||
range.resize(pyr_levels);
|
||||
range_old.resize(pyr_levels);
|
||||
range_inter.resize(pyr_levels);
|
||||
@@ -107,24 +108,21 @@ void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan,
|
||||
|
||||
for (unsigned int i = 0; i < pyr_levels; i++)
|
||||
{
|
||||
s = pow(2.f, int(i));
|
||||
cols_i = ceil(float(width) / float(s));
|
||||
s = std::pow(2.f, int(i));
|
||||
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_old[i].resize(1, cols_i);
|
||||
range[i].assign(0.0f);
|
||||
range_old[i].assign(0.0f);
|
||||
xx[i].resize(1, cols_i);
|
||||
|
||||
xx[i] = Eigen::MatrixXf::Constant(1, cols_i, 0.f);
|
||||
xx_old[i] = Eigen::MatrixXf::Constant(1, cols_i, 0.f);
|
||||
|
||||
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_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_old[i].resize(1, cols_i);
|
||||
yy[i].assign(0.f);
|
||||
yy_old[i].assign(0.f);
|
||||
|
||||
if (cols_i <= cols)
|
||||
{
|
||||
@@ -139,28 +137,31 @@ void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan,
|
||||
normx.resize(1, cols);
|
||||
normy.resize(1, cols);
|
||||
norm_ang.resize(1, cols);
|
||||
weights.setSize(1, cols);
|
||||
null.setSize(1, cols);
|
||||
null.assign(0);
|
||||
cov_odo.assign(0.f);
|
||||
weights.resize(1, cols);
|
||||
|
||||
null = Eigen::MatrixXi::Constant(1, cols, 0);
|
||||
cov_odo = IncrementCov::Zero();
|
||||
|
||||
fps = 1.f; //In Hz
|
||||
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.assign(0.f);
|
||||
kai_loc_old.assign(0.f);
|
||||
kai_abs_ = MatrixS31::Zero();
|
||||
kai_loc_old_ = MatrixS31::Zero();
|
||||
|
||||
module_initialized = true;
|
||||
last_odom_time = ros::Time::now();
|
||||
}
|
||||
|
||||
const mrpt::poses::CPose3D& CLaserOdometry2D::getIncrement() const
|
||||
const CLaserOdometry2D::Pose3d& CLaserOdometry2D::getIncrement() const
|
||||
{
|
||||
return last_increment;
|
||||
return last_increment_;
|
||||
}
|
||||
|
||||
const Eigen::Matrix<float, 3, 3>& CLaserOdometry2D::getIncrementCovariance() const
|
||||
@@ -168,27 +169,27 @@ const Eigen::Matrix<float, 3, 3>& CLaserOdometry2D::getIncrementCovariance() con
|
||||
return cov_odo;
|
||||
}
|
||||
|
||||
mrpt::poses::CPose3D& CLaserOdometry2D::getPose()
|
||||
CLaserOdometry2D::Pose3d& CLaserOdometry2D::getPose()
|
||||
{
|
||||
return robot_pose;
|
||||
return robot_pose_;
|
||||
}
|
||||
|
||||
const mrpt::poses::CPose3D& CLaserOdometry2D::getPose() const
|
||||
const CLaserOdometry2D::Pose3d& CLaserOdometry2D::getPose() const
|
||||
{
|
||||
return robot_pose;
|
||||
return robot_pose_;
|
||||
}
|
||||
|
||||
void CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan)
|
||||
bool CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan)
|
||||
{
|
||||
//==================================================================================
|
||||
// DIFERENTIAL ODOMETRY MULTILEVEL
|
||||
//==================================================================================
|
||||
|
||||
//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();
|
||||
|
||||
m_clock.Tic();
|
||||
createImagePyramid();
|
||||
|
||||
//Coarse-to-fine scheme
|
||||
@@ -198,9 +199,9 @@ void CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan)
|
||||
transformations[i].setIdentity();
|
||||
|
||||
level = i;
|
||||
unsigned int s = pow(2.f,int(ctf_levels-(i+1)));
|
||||
cols_i = ceil(float(cols)/float(s));
|
||||
image_level = ctf_levels - i + round(log2(round(float(width)/float(cols)))) - 1;
|
||||
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
|
||||
if (i == 0)
|
||||
@@ -233,19 +234,31 @@ void CLaserOdometry2D::odometryCalculation(const sensor_msgs::LaserScan& scan)
|
||||
solveSystemNonLinear();
|
||||
//solveSystemOneLevel(); //without robust-function
|
||||
}
|
||||
|
||||
//8. Filter solution
|
||||
filterLevelSolution();
|
||||
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;
|
||||
}
|
||||
|
||||
m_runtime = 1000*m_clock.Tac();
|
||||
//8. Filter solution
|
||||
if (!filterLevelSolution()) return false;
|
||||
}
|
||||
|
||||
ROS_INFO_COND(verbose, "[rf2o] execution time (ms): %f", m_runtime);
|
||||
m_runtime = ros::WallTime::now() - start;
|
||||
|
||||
ROS_INFO_COND(verbose, "[rf2o] execution time (ms): %f",
|
||||
m_runtime.toSec()*double(1000));
|
||||
|
||||
//Update poses
|
||||
PoseUpdate();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void CLaserOdometry2D::createImagePyramid()
|
||||
{
|
||||
@@ -259,13 +272,13 @@ void CLaserOdometry2D::createImagePyramid()
|
||||
//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)
|
||||
|
||||
unsigned int pyr_levels = round(log2(round(float(width)/float(cols)))) + ctf_levels;
|
||||
unsigned int pyr_levels = std::round(std::log2(std::round(float(width)/float(cols)))) + ctf_levels;
|
||||
|
||||
//Generate levels
|
||||
for (unsigned int i = 0; i<pyr_levels; i++)
|
||||
{
|
||||
unsigned int s = pow(2.f,int(i));
|
||||
cols_i = ceil(float(width)/float(s));
|
||||
unsigned int s = std::pow(2.f,int(i));
|
||||
cols_i = std::ceil(float(width)/float(s));
|
||||
|
||||
const unsigned int i_1 = i-1;
|
||||
|
||||
@@ -286,7 +299,7 @@ void CLaserOdometry2D::createImagePyramid()
|
||||
|
||||
for (int l=-2; l<3; l++)
|
||||
{
|
||||
const float abs_dif = abs(range_wf(u+l)-dcenter);
|
||||
const float abs_dif = std::abs(range_wf(u+l)-dcenter);
|
||||
if (abs_dif < max_range_dif)
|
||||
{
|
||||
const float aux_w = g_mask[2+l]*(max_range_dif - abs_dif);
|
||||
@@ -298,7 +311,6 @@ void CLaserOdometry2D::createImagePyramid()
|
||||
}
|
||||
else
|
||||
range[i](u) = 0.f;
|
||||
|
||||
}
|
||||
|
||||
//Boundary
|
||||
@@ -314,7 +326,7 @@ void CLaserOdometry2D::createImagePyramid()
|
||||
const int indu = u+l;
|
||||
if ((indu>=0)&&(indu<cols_i))
|
||||
{
|
||||
const float abs_dif = abs(range_wf(indu)-dcenter);
|
||||
const float abs_dif = std::abs(range_wf(indu)-dcenter);
|
||||
if (abs_dif < max_range_dif)
|
||||
{
|
||||
const float aux_w = g_mask[2+l]*(max_range_dif - abs_dif);
|
||||
@@ -327,7 +339,6 @@ void CLaserOdometry2D::createImagePyramid()
|
||||
}
|
||||
else
|
||||
range[i](u) = 0.f;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -351,7 +362,7 @@ void CLaserOdometry2D::createImagePyramid()
|
||||
|
||||
for (int l=-2; l<3; l++)
|
||||
{
|
||||
const float abs_dif = abs(range[i_1](u2+l)-dcenter);
|
||||
const float abs_dif = std::abs(range[i_1](u2+l)-dcenter);
|
||||
if (abs_dif < max_range_dif)
|
||||
{
|
||||
const float aux_w = g_mask[2+l]*(max_range_dif - abs_dif);
|
||||
@@ -381,7 +392,7 @@ void CLaserOdometry2D::createImagePyramid()
|
||||
const int indu = u2+l;
|
||||
if ((indu>=0)&&(indu<cols_i2))
|
||||
{
|
||||
const float abs_dif = abs(range[i_1](indu)-dcenter);
|
||||
const float abs_dif = std::abs(range[i_1](indu)-dcenter);
|
||||
if (abs_dif < max_range_dif)
|
||||
{
|
||||
const float aux_w = g_mask[2+l]*(max_range_dif - abs_dif);
|
||||
@@ -405,8 +416,8 @@ void CLaserOdometry2D::createImagePyramid()
|
||||
if (range[i](u) > 0.f)
|
||||
{
|
||||
const float tita = -0.5*fovh + float(u)*fovh/float(cols_i-1);
|
||||
xx[i](u) = range[i](u)*cos(tita);
|
||||
yy[i](u) = range[i](u)*sin(tita);
|
||||
xx[i](u) = range[i](u)*std::cos(tita);
|
||||
yy[i](u) = range[i](u)*std::sin(tita);
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -417,8 +428,6 @@ void CLaserOdometry2D::createImagePyramid()
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void CLaserOdometry2D::calculateCoord()
|
||||
{
|
||||
for (unsigned int u = 0; u < cols_i; u++)
|
||||
@@ -438,26 +447,32 @@ void CLaserOdometry2D::calculateCoord()
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void CLaserOdometry2D::calculaterangeDerivativesSurface()
|
||||
{
|
||||
//The gradient size ir reserved at the maximum size (at the constructor)
|
||||
|
||||
//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++)
|
||||
{
|
||||
const float dist = mrpt::math::square(xx_inter[image_level](u+1) - xx_inter[image_level](u))
|
||||
+ mrpt::math::square(yy_inter[image_level](u+1) - yy_inter[image_level](u));
|
||||
float dista = xx_inter[image_level](u+1) - xx_inter[image_level](u);
|
||||
dista *= dista;
|
||||
float distb = yy_inter[image_level](u+1) - yy_inter[image_level](u);
|
||||
distb *= distb;
|
||||
const float dist = dista + distb;
|
||||
|
||||
if (dist > 0.f)
|
||||
rtita(u) = sqrt(dist);
|
||||
rtita(u) = std::sqrt(dist);
|
||||
}
|
||||
|
||||
//Spatial derivatives
|
||||
for (unsigned int u = 1; u < cols_i-1; u++)
|
||||
dtita(u) = (rtita(u-1)*(range_inter[image_level](u+1)-range_inter[image_level](u)) + rtita(u)*(range_inter[image_level](u) - range_inter[image_level](u-1)))/(rtita(u)+rtita(u-1));
|
||||
dtita(u) = (rtita(u-1)*(range_inter[image_level](u+1)-
|
||||
range_inter[image_level](u)) + rtita(u)*(range_inter[image_level](u) -
|
||||
range_inter[image_level](u-1)))/(rtita(u)+rtita(u-1));
|
||||
|
||||
dtita(0) = dtita(1);
|
||||
dtita(cols_i-1) = dtita(cols_i-2);
|
||||
@@ -490,12 +505,11 @@ void CLaserOdometry2D::calculaterangeDerivativesSurface()
|
||||
//dtmed.swap(dt);
|
||||
}
|
||||
|
||||
|
||||
void CLaserOdometry2D::computeNormals()
|
||||
{
|
||||
normx.assign(0.f);
|
||||
normy.assign(0.f);
|
||||
norm_ang.assign(0.f);
|
||||
normx.setConstant(1, cols, 0.f);
|
||||
normy.setConstant(1, cols, 0.f);
|
||||
norm_ang.setConstant(1, cols, 0.f);
|
||||
|
||||
const float incr_tita = fovh/float(cols_i-1);
|
||||
for (unsigned int u=0; u<cols_i; u++)
|
||||
@@ -503,7 +517,7 @@ void CLaserOdometry2D::computeNormals()
|
||||
if (null(u) == 0.f)
|
||||
{
|
||||
const float tita = -0.5f*fovh + float(u)*incr_tita;
|
||||
const float alfa = -atan2(2.f*dtita(u), 2.f*range[image_level](u)*incr_tita);
|
||||
const float alfa = -std::atan2(2.f*dtita(u), 2.f*range[image_level](u)*incr_tita);
|
||||
norm_ang(u) = tita + alfa;
|
||||
if (norm_ang(u) < -M_PI)
|
||||
norm_ang(u) += 2.f*M_PI;
|
||||
@@ -512,21 +526,20 @@ void CLaserOdometry2D::computeNormals()
|
||||
else if (norm_ang(u) > M_PI)
|
||||
norm_ang(u) -= M_PI;
|
||||
|
||||
normx(u) = cos(tita + alfa);
|
||||
normy(u) = sin(tita + alfa);
|
||||
normx(u) = std::cos(tita + alfa);
|
||||
normy(u) = std::sin(tita + alfa);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void CLaserOdometry2D::computeWeights()
|
||||
{
|
||||
//The maximum weight size is reserved at the constructor
|
||||
weights.assign(0.f);
|
||||
weights.setConstant(1, cols, 0.f);
|
||||
|
||||
//Parameters for error_linearization
|
||||
const float kdtita = 1.f;
|
||||
const float kdt = kdtita/mrpt::math::square(fps);
|
||||
const float kdt = kdtita / (fps*fps);
|
||||
const float k2d = 0.2f;
|
||||
|
||||
for (unsigned int u = 1; u < cols_i-1; u++)
|
||||
@@ -540,16 +553,17 @@ void CLaserOdometry2D::computeWeights()
|
||||
const float dtitat = ini_dtita - final_dtita;
|
||||
const float dtita2 = dtita(u+1) - dtita(u-1);
|
||||
|
||||
const float w_der = kdt*mrpt::math::square(dt(u)) + kdtita*mrpt::math::square(dtita(u)) + k2d*(abs(dtitat) + abs(dtita2));
|
||||
const float w_der = kdt*(dt(u)*dt(u)) +
|
||||
kdtita*(dtita(u)*dtita(u)) +
|
||||
k2d*(std::abs(dtitat) + std::abs(dtita2));
|
||||
|
||||
weights(u) = 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;
|
||||
}
|
||||
|
||||
|
||||
void CLaserOdometry2D::findNullPoints()
|
||||
{
|
||||
//Size of null matrix is set to its maximum size (constructor)
|
||||
@@ -567,12 +581,12 @@ void CLaserOdometry2D::findNullPoints()
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Solves the system without considering any robust-function
|
||||
void CLaserOdometry2D::solveSystemOneLevel()
|
||||
{
|
||||
A.resize(num_valid_range, 3);
|
||||
B.setSize(num_valid_range,1);
|
||||
B.resize(num_valid_range, 1);
|
||||
|
||||
unsigned int cont = 0;
|
||||
const float kdtita = (cols_i-1)/fovh;
|
||||
|
||||
@@ -587,9 +601,9 @@ void CLaserOdometry2D::solveSystemOneLevel()
|
||||
const float tita = -0.5*fovh + u/kdtita;
|
||||
|
||||
//Fill the matrix A
|
||||
A(cont, 0) = tw*(cos(tita) + dtita(u)*kdtita*sin(tita)/range_inter[image_level](u));
|
||||
A(cont, 1) = tw*(sin(tita) - dtita(u)*kdtita*cos(tita)/range_inter[image_level](u));
|
||||
A(cont, 2) = tw*(-yy[image_level](u)*cos(tita) + xx[image_level](u)*sin(tita) - dtita(u)*kdtita);
|
||||
A(cont, 0) = tw*(std::cos(tita) + dtita(u)*kdtita*std::sin(tita)/range_inter[image_level](u));
|
||||
A(cont, 1) = tw*(std::sin(tita) - dtita(u)*kdtita*std::cos(tita)/range_inter[image_level](u));
|
||||
A(cont, 2) = tw*(-yy[image_level](u)*std::cos(tita) + xx[image_level](u)*std::sin(tita) - dtita(u)*kdtita);
|
||||
B(cont, 0) = tw*(-dt(u));
|
||||
|
||||
cont++;
|
||||
@@ -597,8 +611,8 @@ void CLaserOdometry2D::solveSystemOneLevel()
|
||||
|
||||
//Solve the linear system of equations using a minimum least squares method
|
||||
Eigen::MatrixXf AtA, AtB;
|
||||
AtA.multiply_AtA(A);
|
||||
AtB.multiply_AtB(A,B);
|
||||
AtA = A.transpose()*A;
|
||||
AtB = A.transpose()*B;
|
||||
Var = AtA.ldlt().solve(AtB);
|
||||
|
||||
//Covariance matrix calculation Cov Order -> vx,vy,wz
|
||||
@@ -606,15 +620,14 @@ void CLaserOdometry2D::solveSystemOneLevel()
|
||||
res = A*Var - B;
|
||||
cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm();
|
||||
|
||||
kai_loc_level = Var;
|
||||
kai_loc_level_ = Var;
|
||||
}
|
||||
|
||||
|
||||
// Solves the system by considering the Cauchy M-estimator robust-function
|
||||
void CLaserOdometry2D::solveSystemNonLinear()
|
||||
{
|
||||
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;
|
||||
const float kdtita = float(cols_i-1)/fovh;
|
||||
|
||||
@@ -629,9 +642,9 @@ void CLaserOdometry2D::solveSystemNonLinear()
|
||||
const float tita = -0.5*fovh + u/kdtita;
|
||||
|
||||
//Fill the matrix A
|
||||
A(cont, 0) = tw*(cos(tita) + dtita(u)*kdtita*sin(tita)/range_inter[image_level](u));
|
||||
A(cont, 1) = tw*(sin(tita) - dtita(u)*kdtita*cos(tita)/range_inter[image_level](u));
|
||||
A(cont, 2) = tw*(-yy[image_level](u)*cos(tita) + xx[image_level](u)*sin(tita) - dtita(u)*kdtita);
|
||||
A(cont, 0) = tw*(std::cos(tita) + dtita(u)*kdtita*std::sin(tita)/range_inter[image_level](u));
|
||||
A(cont, 1) = tw*(std::sin(tita) - dtita(u)*kdtita*std::cos(tita)/range_inter[image_level](u));
|
||||
A(cont, 2) = tw*(-yy[image_level](u)*std::cos(tita) + xx[image_level](u)*std::sin(tita) - dtita(u)*kdtita);
|
||||
B(cont, 0) = tw*(-dt(u));
|
||||
|
||||
cont++;
|
||||
@@ -639,8 +652,8 @@ void CLaserOdometry2D::solveSystemNonLinear()
|
||||
|
||||
//Solve the linear system of equations using a minimum least squares method
|
||||
Eigen::MatrixXf AtA, AtB;
|
||||
AtA.multiply_AtA(A);
|
||||
AtB.multiply_AtB(A,B);
|
||||
AtA = A.transpose()*A;
|
||||
AtB = A.transpose()*B;
|
||||
Var = AtA.ldlt().solve(AtB);
|
||||
|
||||
//Covariance matrix calculation Cov Order -> vx,vy,wz
|
||||
@@ -655,8 +668,8 @@ void CLaserOdometry2D::solveSystemNonLinear()
|
||||
for (unsigned int u = 1; u < cols_i-1; u++)
|
||||
if (null(u) == 0)
|
||||
{
|
||||
aver_dt += fabsf(dt(u));
|
||||
aver_res += fabsf(res(ind++));
|
||||
aver_dt += std::abs(dt(u));
|
||||
aver_res += std::abs(res(ind++));
|
||||
}
|
||||
aver_dt /= cont; aver_res /= cont;
|
||||
// printf("\n Aver dt = %f, aver res = %f", aver_dt, aver_res);
|
||||
@@ -677,7 +690,7 @@ void CLaserOdometry2D::solveSystemNonLinear()
|
||||
for (unsigned int u = 1; u < cols_i-1; u++)
|
||||
if (null(u) == 0)
|
||||
{
|
||||
const float res_weight = sqrt(1.f/(1.f + mrpt::math::square(k*res(cont))));
|
||||
const float res_weight = std::sqrt(1.f/(1.f + ((k*res(cont))*(k*res(cont)))));
|
||||
|
||||
//Fill the matrix Aw
|
||||
Aw(cont,0) = res_weight*A(cont,0);
|
||||
@@ -688,8 +701,8 @@ void CLaserOdometry2D::solveSystemNonLinear()
|
||||
}
|
||||
|
||||
//Solve the linear system of equations using a minimum least squares method
|
||||
AtA.multiply_AtA(Aw);
|
||||
AtB.multiply_AtB(Aw,Bw);
|
||||
AtA = Aw.transpose()*Aw;
|
||||
AtB = Aw.transpose()*Bw;
|
||||
Var = AtA.ldlt().solve(AtB);
|
||||
res = A*Var - B;
|
||||
|
||||
@@ -701,24 +714,21 @@ void CLaserOdometry2D::solveSystemNonLinear()
|
||||
}
|
||||
|
||||
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: " << cov_odo);
|
||||
ROS_INFO_STREAM_COND(verbose && false, "[rf2o] COV_ODO:\n" << cov_odo);
|
||||
}
|
||||
|
||||
void CLaserOdometry2D::Reset(mrpt::poses::CPose3D ini_pose, CObservation2DRangeScan scan)
|
||||
void CLaserOdometry2D::Reset(const Pose3d& ini_pose/*, CObservation2DRangeScan scan*/)
|
||||
{
|
||||
//Set the initial pose
|
||||
laser_pose = ini_pose;
|
||||
laser_oldpose = ini_pose;
|
||||
laser_pose_ = ini_pose;
|
||||
laser_oldpose_ = ini_pose;
|
||||
|
||||
//readLaser(scan);
|
||||
createImagePyramid();
|
||||
//readLaser(scan);
|
||||
createImagePyramid();
|
||||
}
|
||||
|
||||
|
||||
void CLaserOdometry2D::performWarping()
|
||||
{
|
||||
Eigen::Matrix3f acu_trans;
|
||||
@@ -727,9 +737,9 @@ void CLaserOdometry2D::performWarping()
|
||||
for (unsigned int i=1; i<=level; i++)
|
||||
acu_trans = transformations[i-1]*acu_trans;
|
||||
|
||||
Eigen::MatrixXf wacu(1,cols_i);
|
||||
wacu.assign(0.f);
|
||||
range_warped[image_level].assign(0.f);
|
||||
Eigen::MatrixXf wacu = Eigen::MatrixXf::Constant(1, cols_i, 0.f);
|
||||
|
||||
range_warped[image_level].setConstant(1, cols_i, 0.f);
|
||||
|
||||
const float cols_lim = float(cols_i-1);
|
||||
const float kdtita = cols_lim/fovh;
|
||||
@@ -741,8 +751,8 @@ void CLaserOdometry2D::performWarping()
|
||||
//Transform point to the warped reference frame
|
||||
const float x_w = acu_trans(0,0)*xx[image_level](j) + acu_trans(0,1)*yy[image_level](j) + acu_trans(0,2);
|
||||
const float y_w = acu_trans(1,0)*xx[image_level](j) + acu_trans(1,1)*yy[image_level](j) + acu_trans(1,2);
|
||||
const float tita_w = atan2(y_w, x_w);
|
||||
const float range_w = sqrt(x_w*x_w + y_w*y_w);
|
||||
const float tita_w = std::atan2(y_w, x_w);
|
||||
const float range_w = std::sqrt(x_w*x_w + y_w*y_w);
|
||||
|
||||
//Calculate warping
|
||||
const float uwarp = kdtita*(tita_w + 0.5*fovh);
|
||||
@@ -756,18 +766,18 @@ void CLaserOdometry2D::performWarping()
|
||||
const float delta_l = uwarp - float(uwarp_l);
|
||||
|
||||
//Very close pixel
|
||||
if (abs(round(uwarp) - uwarp) < 0.05f)
|
||||
if (std::abs(std::round(uwarp) - uwarp) < 0.05f)
|
||||
{
|
||||
range_warped[image_level](round(uwarp)) += range_w;
|
||||
wacu(round(uwarp)) += 1.f;
|
||||
wacu(std::round(uwarp)) += 1.f;
|
||||
}
|
||||
else
|
||||
{
|
||||
const float w_r = mrpt::math::square(delta_l);
|
||||
const float w_r = delta_l*delta_l;
|
||||
range_warped[image_level](uwarp_r) += w_r*range_w;
|
||||
wacu(uwarp_r) += w_r;
|
||||
|
||||
const float w_l = mrpt::math::square(delta_r);
|
||||
const float w_l = delta_r*delta_r;
|
||||
range_warped[image_level](uwarp_l) += w_l*range_w;
|
||||
wacu(uwarp_l) += w_l;
|
||||
}
|
||||
@@ -782,8 +792,8 @@ void CLaserOdometry2D::performWarping()
|
||||
{
|
||||
const float tita = -0.5f*fovh + float(u)/kdtita;
|
||||
range_warped[image_level](u) /= wacu(u);
|
||||
xx_warped[image_level](u) = range_warped[image_level](u)*cos(tita);
|
||||
yy_warped[image_level](u) = range_warped[image_level](u)*sin(tita);
|
||||
xx_warped[image_level](u) = range_warped[image_level](u)*std::cos(tita);
|
||||
yy_warped[image_level](u) = range_warped[image_level](u)*std::sin(tita);
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -794,19 +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
|
||||
@@ -815,11 +821,13 @@ void CLaserOdometry2D::filterLevelSolution()
|
||||
Eigen::Matrix<float,3,1> kai_b;
|
||||
Bii = eigensolver.eigenvectors();
|
||||
|
||||
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
|
||||
//-------------------------------------------------------------------------------------------------
|
||||
mrpt::math::CMatrixFloat31 kai_loc_sub;
|
||||
MatrixS31 kai_loc_sub;
|
||||
|
||||
//Important: we have to substract the solutions from previous levels
|
||||
Eigen::Matrix3f acu_trans;
|
||||
@@ -833,42 +841,45 @@ void CLaserOdometry2D::filterLevelSolution()
|
||||
kai_loc_sub(2) = 0.f;
|
||||
else
|
||||
{
|
||||
#if MRPT_VERSION>=0x130
|
||||
kai_loc_sub(2) = -fps*acos(acu_trans(0,0))*mrpt::utils::sign(acu_trans(1,0));
|
||||
#else
|
||||
kai_loc_sub(2) = -fps*acos(acu_trans(0,0))*mrpt::math::sign(acu_trans(1,0));
|
||||
#endif
|
||||
kai_loc_sub(2) = -fps*std::acos(acu_trans(0,0))*rf2o::sign(acu_trans(1,0));
|
||||
}
|
||||
kai_loc_sub += kai_loc_old;
|
||||
kai_loc_sub += kai_loc_old_;
|
||||
|
||||
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*expf(-int(level)), df = 0.05f*expf(-int(level));
|
||||
const float cf = 15e3f*std::exp(-float(int(level))),
|
||||
df = 0.05f*std::exp(-float(int(level)));
|
||||
|
||||
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) = cos(rot);
|
||||
transformations[level](0,1) = -sin(rot);
|
||||
transformations[level](1,0) = sin(rot);
|
||||
transformations[level](1,1) = cos(rot);
|
||||
|
||||
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()
|
||||
{
|
||||
@@ -876,57 +887,65 @@ 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);
|
||||
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;
|
||||
kai_loc_(2) = 0.f;
|
||||
else
|
||||
{
|
||||
#if MRPT_VERSION>=0x130
|
||||
kai_loc(2) = fps*acos(acu_trans(0,0))*mrpt::utils::sign(acu_trans(1,0));
|
||||
#else
|
||||
kai_loc(2) = fps*acos(acu_trans(0,0))*mrpt::math::sign(acu_trans(1,0));
|
||||
#endif
|
||||
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 = laser_pose.yaw();
|
||||
//cout << endl << "Arc cos (incr tita): " << kai_loc_(2);
|
||||
|
||||
kai_abs(0) = kai_loc(0)*cos(phi) - kai_loc(1)*sin(phi);
|
||||
kai_abs(1) = kai_loc(0)*sin(phi) + kai_loc(1)*cos(phi);
|
||||
kai_abs(2) = kai_loc(2);
|
||||
float phi = rf2o::getYaw(laser_pose_.rotation());
|
||||
|
||||
kai_abs_(0) = kai_loc_(0)*std::cos(phi) - kai_loc_(1)*std::sin(phi);
|
||||
kai_abs_(1) = kai_loc_(0)*std::sin(phi) + kai_loc_(1)*std::cos(phi);
|
||||
kai_abs_(2) = kai_loc_(2);
|
||||
|
||||
|
||||
// Update poses
|
||||
//-------------------------------------------------------
|
||||
laser_oldpose = laser_pose;
|
||||
laser_oldpose_ = laser_pose_;
|
||||
|
||||
mrpt::math::CMatrixDouble33 aux_acu = acu_trans;
|
||||
mrpt::poses::CPose2D pose_aux_2D(acu_trans(0,2), acu_trans(1,2), kai_loc(2)/fps);
|
||||
laser_pose = laser_pose + mrpt::poses::CPose3D(pose_aux_2D);
|
||||
// Eigen::Matrix3f aux_acu = acu_trans;
|
||||
Pose3d pose_aux_2D = Pose3d::Identity();
|
||||
|
||||
last_increment = pose_aux_2D;
|
||||
pose_aux_2D = rf2o::matrixYaw(double(kai_loc_(2)/fps));
|
||||
pose_aux_2D.translation()(0) = acu_trans(0,2);
|
||||
pose_aux_2D.translation()(1) = acu_trans(1,2);
|
||||
|
||||
laser_pose_ = laser_pose_ * pose_aux_2D;
|
||||
|
||||
last_increment_ = pose_aux_2D;
|
||||
|
||||
// Compute kai_loc_old
|
||||
//-------------------------------------------------------
|
||||
phi = laser_pose.yaw();
|
||||
kai_loc_old(0) = kai_abs(0)*cos(phi) + kai_abs(1)*sin(phi);
|
||||
kai_loc_old(1) = -kai_abs(0)*sin(phi) + kai_abs(1)*cos(phi);
|
||||
kai_loc_old(2) = kai_abs(2);
|
||||
phi = rf2o::getYaw(laser_pose_.rotation());
|
||||
kai_loc_old_(0) = kai_abs_(0)*std::cos(phi) + kai_abs_(1)*std::sin(phi);
|
||||
kai_loc_old_(1) = -kai_abs_(0)*std::sin(phi) + kai_abs_(1)*std::cos(phi);
|
||||
kai_loc_old_(2) = kai_abs_(2);
|
||||
|
||||
ROS_INFO_COND(verbose, "[rf2o] LASERodom = [%f %f %f]",laser_pose.x(),laser_pose.y(),laser_pose.yaw());
|
||||
ROS_INFO_COND(verbose, "[rf2o] LASERodom = [%f %f %f]",
|
||||
laser_pose_.translation()(0),
|
||||
laser_pose_.translation()(1),
|
||||
rf2o::getYaw(laser_pose_.rotation()));
|
||||
|
||||
//Compose Transformations
|
||||
robot_pose = laser_pose + laser_pose_on_robot_inv;
|
||||
ROS_INFO_COND(verbose, "BASEodom = [%f %f %f]",robot_pose.x(),robot_pose.y(),robot_pose.yaw());
|
||||
robot_pose_ = laser_pose_ * laser_pose_on_robot_inv_;
|
||||
|
||||
ROS_INFO_COND(verbose, "BASEodom = [%f %f %f]",
|
||||
robot_pose_.translation()(0),
|
||||
robot_pose_.translation()(1),
|
||||
rf2o::getYaw(robot_pose_.rotation()));
|
||||
|
||||
// Estimate linear/angular speeds (mandatory for base_local_planner)
|
||||
// last_scan -> the last scan received
|
||||
@@ -936,13 +955,17 @@ void CLaserOdometry2D::PoseUpdate()
|
||||
last_odom_time = current_scan_time;
|
||||
lin_speed = acu_trans(0,2) / time_inc_sec;
|
||||
//double lin_speed = sqrt( mrpt::math::square(robot_oldpose.x()-robot_pose.x()) + mrpt::math::square(robot_oldpose.y()-robot_pose.y()) )/time_inc_sec;
|
||||
double ang_inc = robot_pose.yaw() - robot_oldpose.yaw();
|
||||
|
||||
double ang_inc = rf2o::getYaw(robot_pose_.rotation()) -
|
||||
rf2o::getYaw(robot_oldpose_.rotation());
|
||||
|
||||
if (ang_inc > 3.14159)
|
||||
ang_inc -= 2*3.14159;
|
||||
if (ang_inc < -3.14159)
|
||||
ang_inc += 2*3.14159;
|
||||
|
||||
ang_speed = ang_inc/time_inc_sec;
|
||||
robot_oldpose = robot_pose;
|
||||
robot_oldpose_ = robot_pose_;
|
||||
|
||||
//filter speeds
|
||||
/*
|
||||
@@ -959,3 +982,5 @@ void CLaserOdometry2D::PoseUpdate()
|
||||
ang_speed = sum2 / last_m_ang_speeds.size();
|
||||
*/
|
||||
}
|
||||
|
||||
} /* namespace rf2o */
|
||||
|
||||
@@ -11,6 +11,8 @@
|
||||
*
|
||||
* Maintainer: Javier G. Monroy
|
||||
* MAPIR group: http://mapir.isa.uma.es/
|
||||
*
|
||||
* Modifications: Jeremie Deray
|
||||
******************************************************************************************** */
|
||||
|
||||
#include "rf2o_laser_odometry/CLaserOdometry2D.h"
|
||||
@@ -18,12 +20,14 @@
|
||||
#include <tf/transform_broadcaster.h>
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
namespace rf2o {
|
||||
|
||||
class CLaserOdometry2DNode : CLaserOdometry2D
|
||||
{
|
||||
public:
|
||||
|
||||
CLaserOdometry2DNode();
|
||||
~CLaserOdometry2DNode();
|
||||
~CLaserOdometry2DNode() = default;
|
||||
|
||||
void process(const ros::TimerEvent &);
|
||||
void publish();
|
||||
@@ -109,11 +113,6 @@ CLaserOdometry2DNode::CLaserOdometry2DNode() :
|
||||
ROS_INFO_STREAM("Listening laser scan from topic: " << laser_sub.getTopic());
|
||||
}
|
||||
|
||||
CLaserOdometry2DNode::~CLaserOdometry2DNode()
|
||||
{
|
||||
//
|
||||
}
|
||||
|
||||
bool CLaserOdometry2DNode::setLaserPoseFromTf()
|
||||
{
|
||||
bool retrieved = false;
|
||||
@@ -134,18 +133,21 @@ bool CLaserOdometry2DNode::setLaserPoseFromTf()
|
||||
retrieved = false;
|
||||
}
|
||||
|
||||
//TF:transform -> mrpt::CPose3D (see mrpt-ros-bridge)
|
||||
mrpt::poses::CPose3D laser_tf;
|
||||
const tf::Vector3 &t = transform.getOrigin();
|
||||
laser_tf.x() = t[0];
|
||||
laser_tf.y() = t[1];
|
||||
laser_tf.z() = t[2];
|
||||
//TF:transform -> Eigen::Isometry3d
|
||||
|
||||
const tf::Matrix3x3 &basis = transform.getBasis();
|
||||
mrpt::math::CMatrixDouble33 R;
|
||||
Eigen::Matrix3d R;
|
||||
|
||||
for(int r = 0; r < 3; r++)
|
||||
for(int c = 0; c < 3; c++)
|
||||
R(r,c) = basis[r][c];
|
||||
laser_tf.setRotationMatrix(R);
|
||||
|
||||
Pose3d laser_tf(R);
|
||||
|
||||
const tf::Vector3 &t = transform.getOrigin();
|
||||
laser_tf.translation()(0) = t[0];
|
||||
laser_tf.translation()(1) = t[1];
|
||||
laser_tf.translation()(2) = t[2];
|
||||
|
||||
setLaserPose(laser_tf);
|
||||
|
||||
@@ -194,7 +196,7 @@ void CLaserOdometry2DNode::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr&
|
||||
}
|
||||
else
|
||||
{
|
||||
Init(last_scan, initial_robot_pose.pose.pose);
|
||||
init(last_scan, initial_robot_pose.pose.pose);
|
||||
first_laser_scan = false;
|
||||
}
|
||||
}
|
||||
@@ -221,10 +223,10 @@ void CLaserOdometry2DNode::publish()
|
||||
odom_trans.header.stamp = ros::Time::now();
|
||||
odom_trans.header.frame_id = odom_frame_id;
|
||||
odom_trans.child_frame_id = base_frame_id;
|
||||
odom_trans.transform.translation.x = robot_pose.x();
|
||||
odom_trans.transform.translation.y = robot_pose.y();
|
||||
odom_trans.transform.translation.x = robot_pose_.translation()(0);
|
||||
odom_trans.transform.translation.y = robot_pose_.translation()(1);
|
||||
odom_trans.transform.translation.z = 0.0;
|
||||
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(robot_pose.yaw());
|
||||
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(rf2o::getYaw(robot_pose_.rotation()));
|
||||
//send the transform
|
||||
odom_broadcaster.sendTransform(odom_trans);
|
||||
}
|
||||
@@ -236,10 +238,10 @@ void CLaserOdometry2DNode::publish()
|
||||
odom.header.stamp = ros::Time::now();
|
||||
odom.header.frame_id = odom_frame_id;
|
||||
//set the position
|
||||
odom.pose.pose.position.x = robot_pose.x();
|
||||
odom.pose.pose.position.y = robot_pose.y();
|
||||
odom.pose.pose.position.x = robot_pose_.translation()(0);
|
||||
odom.pose.pose.position.y = robot_pose_.translation()(1);
|
||||
odom.pose.pose.position.z = 0.0;
|
||||
odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(robot_pose.yaw());
|
||||
odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(rf2o::getYaw(robot_pose_.rotation()));
|
||||
//set the velocity
|
||||
odom.child_frame_id = base_frame_id;
|
||||
odom.twist.twist.linear.x = lin_speed; //linear speed
|
||||
@@ -249,6 +251,8 @@ void CLaserOdometry2DNode::publish()
|
||||
odom_pub.publish(odom);
|
||||
}
|
||||
|
||||
} /* namespace rf2o */
|
||||
|
||||
//-----------------------------------------------------------------------------------
|
||||
// MAIN
|
||||
//-----------------------------------------------------------------------------------
|
||||
@@ -256,7 +260,7 @@ int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "RF2O_LaserOdom");
|
||||
|
||||
CLaserOdometry2DNode myLaserOdomNode;
|
||||
rf2o::CLaserOdometry2DNode myLaserOdomNode;
|
||||
|
||||
ros::TimerOptions timer_opt;
|
||||
timer_opt.oneshot = false;
|
||||
@@ -264,7 +268,7 @@ int main(int argc, char** argv)
|
||||
timer_opt.callback_queue = ros::getGlobalCallbackQueue();
|
||||
timer_opt.tracked_object = ros::VoidConstPtr();
|
||||
|
||||
timer_opt.callback = boost::bind(&CLaserOdometry2DNode::process, &myLaserOdomNode, _1);
|
||||
timer_opt.callback = boost::bind(&rf2o::CLaserOdometry2DNode::process, &myLaserOdomNode, _1);
|
||||
timer_opt.period = ros::Rate(myLaserOdomNode.freq).expectedCycleTime();
|
||||
|
||||
ros::Timer rf2o_timer = ros::NodeHandle("~").createTimer(timer_opt);
|
||||
|
||||
Reference in New Issue
Block a user