i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-11 20:39:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

simplified parameters

Browse Source
This commit is contained in:
matlabbe
2026-10-10 18:26:20 -07:00
parent 874bc35d22
commit de1761bd6d
12 changed files with 250 additions and 143 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/ImuMotionPredictor.h
+9 -5
View File
@@ -66,16 +66,20 @@ class RTABMAP_CORE_EXPORT ImuMotionPredictor
{ {
public: public:
/** /**
* Without acceleration in the IMU samples, the position follows the last velocity * The IMU acceleration is used only with a velocity window (> 0): over a single frame,
* the velocity is too noisy to be carried forward with it. Without it (window of 0, or
* no acceleration in the IMU samples), the position follows the last velocity
* (constant velocity model). * (constant velocity model).
* *
* @param maxPoseInterval odometry poses older (s) than this are not used to estimate * @param maxPoseInterval odometry poses older (s) than this are not used to estimate
* the velocity, which is null without one * the velocity, which is null without one
* @param velocityWindow the velocity is estimated from the displacement since the * @param velocityWindow the velocity is estimated from the displacement since the
* newest pose at least this old (s). Over a single frame * newest pose at least this old (s), corrected by the
* interval, the noise of the odometry poses would be of the order * acceleration measured since, so that it is the velocity at the
* of the velocity itself; with the acceleration, a longer window * last pose, not an average. Over a single frame interval, the
* still gives the velocity at the last pose, not an average. * noise of the odometry poses would be of the order of the
* velocity itself. 0: the displacement since the previous pose,
* without acceleration.
* @param gravity magnitude (m/s^2) of the gravity removed from the specific force * @param gravity magnitude (m/s^2) of the gravity removed from the specific force
* given to addImu(), standard gravity by default * given to addImu(), standard gravity by default
* *
corelib/include/rtabmap/core/Odometry.h
+1 -2
View File
@@ -160,7 +160,6 @@ private:
bool _force3DoF; bool _force3DoF;
bool _holonomic; bool _holonomic;
bool guessFromMotion_; bool guessFromMotion_;
bool guessImuAcceleration_;
float guessSmoothingDelay_; float guessSmoothingDelay_;
int _filteringStrategy; int _filteringStrategy;
int _particleSize; int _particleSize;
@@ -191,7 +190,7 @@ private:
std::vector<StereoCameraModel> stereoModels_; std::vector<StereoCameraModel> stereoModels_;
std::vector<CameraModel> models_; std::vector<CameraModel> models_;
std::map<double, Transform> imus_; std::map<double, Transform> imus_;
ImuMotionPredictor imuMotionPredictor_; // used with Odom/GuessImuAcceleration only ImuMotionPredictor imuMotionPredictor_; // fed when IMU is received (motion guess and deskewing)
}; };
} /* namespace rtabmap */ } /* namespace rtabmap */
corelib/include/rtabmap/core/Parameters.h
+3 -4
View File
@@ -512,15 +512,14 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Odom, KalmanProcessNoise, float, 0.001, "Process noise covariance value."); RTABMAP_PARAM(Odom, KalmanProcessNoise, float, 0.001, "Process noise covariance value.");
RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01, "Process measurement covariance value."); RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01, "Process measurement covariance value.");
RTABMAP_PARAM(Odom, GuessMotion, bool, true, "Guess next transformation from the last motion computed."); RTABMAP_PARAM(Odom, GuessMotion, bool, true, "Guess next transformation from the last motion computed.");
RTABMAP_PARAM(Odom, GuessImuAcceleration, bool, false, uFormat("With an IMU giving orientation and linear acceleration, predict the translation of the motion guess (with \"%s\") by integrating the IMU acceleration from the velocity at the previous frame, instead of with a constant velocity. That velocity is the average over \"%s\" (from the displacement over that delay), carried to the previous frame with the acceleration measured since: it is not delayed like the average alone. Set \"%s\" to about 0.5 s: over a single frame, the noise of the poses is of the order of the velocity itself. Ignored by odometry approaches processing the IMU themselves.", kOdomGuessMotion().c_str(), kOdomGuessSmoothingDelay().c_str(), kOdomGuessSmoothingDelay().c_str())); RTABMAP_PARAM(Odom, ImuGravity, float, 9.80665, uFormat("Gravity magnitude (m/s^2) removed from the IMU linear acceleration (used with \"%s\" > 0). Standard gravity by default. Set it to what the accelerometer reads at rest to compensate for its scale error, or for another gravity than Earth's.", kOdomGuessSmoothingDelay().c_str()));
RTABMAP_PARAM(Odom, ImuGravity, float, 9.80665, uFormat("Gravity magnitude (m/s^2) removed from the IMU linear acceleration with \"%s\". Standard gravity by default. Set it to what the accelerometer reads at rest to compensate for its scale error, or for another gravity than Earth's.", kOdomGuessImuAcceleration().c_str())); RTABMAP_PARAM(Odom, GuessSmoothingDelay, float, 0, uFormat("Guess smoothing delay (s). Estimated velocity is averaged based on last transforms up to this maximum delay. This can help to get smoother velocity prediction. Last velocity computed is used directly if \"%s\" is set or the delay is below the odometry rate. With an IMU giving orientation and linear acceleration, a delay > 0 also enables the IMU acceleration: the velocity is then the displacement over this delay corrected by the acceleration measured since, so that it is the velocity at the last frame rather than a delayed average, and the motion guess and lidar deskewing (\"%s\") integrate the acceleration from it. Recommended (~0.5 s) with an IMU. With 0, the IMU only gives the orientation. Without IMU, the average is delayed by half this delay: it can make sense for platforms with inertia (e.g., cars at high speed, where one bad frame would otherwise change the predicted velocity a lot), for high frame rates, or when the velocity is used for lidar deskewing, where its noise would feed back into the next poses.", kOdomFilteringStrategy().c_str(), kOdomDeskewing().c_str()));
RTABMAP_PARAM(Odom, GuessSmoothingDelay, float, 0, uFormat("Guess smoothing delay (s). Estimated velocity is averaged based on last transforms up to this maximum delay. This can help to get smoother velocity prediction. Last velocity computed is used directly if \"%s\" is set or the delay is below the odometry rate. Recommended (~0.5 s) with \"%s\": the translational velocity is then the displacement over this delay corrected by the IMU acceleration, so that it is the velocity at the last frame rather than a delayed average. Without IMU, the average is delayed by half this delay: it can make sense for platforms with inertia (e.g., cars at high speed, where one bad frame would otherwise change the predicted velocity a lot), for high frame rates, or when the velocity is used for lidar deskewing (\"%s\"), where its noise would feed back into the next poses.", kOdomFilteringStrategy().c_str(), kOdomGuessImuAcceleration().c_str(), kOdomDeskewing().c_str()));
RTABMAP_PARAM(Odom, KeyFrameThr, float, 0.3, "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame."); RTABMAP_PARAM(Odom, KeyFrameThr, float, 0.3, "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
RTABMAP_PARAM(Odom, VisKeyFrameThr, int, 150, "[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame."); RTABMAP_PARAM(Odom, VisKeyFrameThr, int, 150, "[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
RTABMAP_PARAM(Odom, ScanKeyFrameThr, float, 0.9, "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame."); RTABMAP_PARAM(Odom, ScanKeyFrameThr, float, 0.9, "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
RTABMAP_PARAM(Odom, ImageDecimation, unsigned int, 1, uFormat("Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.", kVisDepthAsMask().c_str())); RTABMAP_PARAM(Odom, ImageDecimation, unsigned int, 1, uFormat("Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.", kVisDepthAsMask().c_str()));
RTABMAP_PARAM(Odom, AlignWithGround, bool, false, "Align odometry with the ground on initialization."); RTABMAP_PARAM(Odom, AlignWithGround, bool, false, "Align odometry with the ground on initialization.");
RTABMAP_PARAM(Odom, Deskewing, bool, true, "Lidar deskewing. If input lidar has time channel, it will be deskewed with a constant motion model (with IMU orientation and/or guess if provided)."); RTABMAP_PARAM(Odom, Deskewing, bool, true, uFormat("Lidar deskewing. If input lidar has time channel, it will be deskewed. With an IMU, the pose of every point is predicted from the previous frame: orientation from the IMU, translation from the velocity (with the IMU acceleration if \"%s\" > 0). Without IMU, with a constant motion model (or the guess if provided).", kOdomGuessSmoothingDelay().c_str()));
// Odometry Frame-to-Map // Odometry Frame-to-Map
RTABMAP_PARAM(OdomF2M, MaxSize, int, 2000, "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words."); RTABMAP_PARAM(OdomF2M, MaxSize, int, 2000, "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
corelib/include/rtabmap/core/util3d.h
+22
View File
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef UTIL3D_H_ #ifndef UTIL3D_H_
#define UTIL3D_H_ #define UTIL3D_H_
#include <functional>
#include "rtabmap/core/rtabmap_core_export.h" #include "rtabmap/core/rtabmap_core_export.h"
#include <pcl/point_cloud.h> #include <pcl/point_cloud.h>
@@ -1295,6 +1296,27 @@ LaserScan RTABMAP_CORE_EXPORT deskew(
double inputStamp, double inputStamp,
const rtabmap::Transform & velocity); const rtabmap::Transform & velocity);
/**
* @brief Deskews a lidar scan with a motion given by the caller.
*
* Same as the velocity overload, but the motion during the sweep comes from @p motion,
* for instance a pose predicted from an IMU.
*
* @param input scan with a time channel (`kXYZIT` or `kXYZIRT`)
* @param inputStamp stamp of the scan, which the time channel is relative to
* @param motion for a stamp (s) in the sweep, the pose of the scan's base frame at that
* stamp relative to the base frame at @p inputStamp; a null transform
* aborts deskewing
* @param slerp call @p motion only for the first and last points and interpolate in
* between, instead of calling it for every time of the sweep
* @return the deskewed scan, empty on error
*/
LaserScan RTABMAP_CORE_EXPORT deskew(
const LaserScan & input,
double inputStamp,
const std::function<rtabmap::Transform(double stamp)> & motion,
bool slerp = false);
} // namespace util3d } // namespace util3d
} // namespace rtabmap } // namespace rtabmap
corelib/src/ImuMotionPredictor.cpp
+8 -3
View File
@@ -133,7 +133,8 @@ void ImuMotionPredictor::addPose(double stamp, const rtabmap::Transform & pose)
pose.y() - reference->second.y(), pose.y() - reference->second.y(),
pose.z() - reference->second.z()); pose.z() - reference->second.z());
if(!samples_.empty() && if(velocityWindow_ > 0.0 &&
!samples_.empty() &&
samples_.begin()->first <= reference->first && samples_.begin()->first <= reference->first &&
samples_.rbegin()->first >= stamp) samples_.rbegin()->first >= stamp)
{ {
@@ -147,7 +148,7 @@ void ImuMotionPredictor::addPose(double stamp, const rtabmap::Transform & pose)
} }
else else
{ {
// Average velocity over the interval (the samples don't cover it) // Average velocity over the interval (no window, or the samples don't cover it)
velocity = displacement / interval; velocity = displacement / interval;
} }
} }
@@ -202,7 +203,11 @@ rtabmap::Transform ImuMotionPredictor::predict(double stamp) const
// D(t) is the displacement due to the change of velocity since t0, V(s) = integral_t0^s a(u) du. // D(t) is the displacement due to the change of velocity since t0, V(s) = integral_t0^s a(u) du.
Eigen::Vector3d position(pose_.x(), pose_.y(), pose_.z()); // p0 Eigen::Vector3d position(pose_.x(), pose_.y(), pose_.z()); // p0
position += velocity_ * (stamp - poseStamp_); // + v0*(t-t0) position += velocity_ * (stamp - poseStamp_); // + v0*(t-t0)
if(stamp >= poseStamp_) if(velocityWindow_ <= 0.0)
{
// No acceleration without a velocity window: constant velocity
}
else if(stamp >= poseStamp_)
{ {
// + D(t). D and V are kept at every sample stamp since t0 (integrated_), so only // + D(t). D and V are kept at every sample stamp since t0 (integrated_), so only
// the part from the last sample ti <= t is integrated here, with dt = t-ti: // the part from the last sample ti <= t is integrated here, with dt = t-ti:
corelib/src/Odometry.cpp
+44 -47
View File
@@ -134,7 +134,6 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_force3DoF(Parameters::defaultRegForce3DoF()), _force3DoF(Parameters::defaultRegForce3DoF()),
_holonomic(Parameters::defaultOdomHolonomic()), _holonomic(Parameters::defaultOdomHolonomic()),
guessFromMotion_(Parameters::defaultOdomGuessMotion()), guessFromMotion_(Parameters::defaultOdomGuessMotion()),
guessImuAcceleration_(Parameters::defaultOdomGuessImuAcceleration()),
guessSmoothingDelay_(Parameters::defaultOdomGuessSmoothingDelay()), guessSmoothingDelay_(Parameters::defaultOdomGuessSmoothingDelay()),
_filteringStrategy(Parameters::defaultOdomFilteringStrategy()), _filteringStrategy(Parameters::defaultOdomFilteringStrategy()),
_particleSize(Parameters::defaultOdomParticleSize()), _particleSize(Parameters::defaultOdomParticleSize()),
@@ -161,13 +160,12 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kRegForce3DoF(), _force3DoF); Parameters::parse(parameters, Parameters::kRegForce3DoF(), _force3DoF);
Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic); Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_); Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
Parameters::parse(parameters, Parameters::kOdomGuessImuAcceleration(), guessImuAcceleration_);
Parameters::parse(parameters, Parameters::kOdomGuessSmoothingDelay(), guessSmoothingDelay_); Parameters::parse(parameters, Parameters::kOdomGuessSmoothingDelay(), guessSmoothingDelay_);
if(guessImuAcceleration_)
{ {
float imuGravity = Parameters::defaultOdomImuGravity(); float imuGravity = Parameters::defaultOdomImuGravity();
Parameters::parse(parameters, Parameters::kOdomImuGravity(), imuGravity); Parameters::parse(parameters, Parameters::kOdomImuGravity(), imuGravity);
// The velocity is estimated over the smoothing delay // The velocity is estimated over the smoothing delay, and the IMU acceleration used
// only with one (> 0)
imuMotionPredictor_ = ImuMotionPredictor(1.0, guessSmoothingDelay_, imuGravity); imuMotionPredictor_ = ImuMotionPredictor(1.0, guessSmoothingDelay_, imuGravity);
} }
Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData); Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
@@ -351,11 +349,8 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
imus_.erase(imus_.begin()); imus_.erase(imus_.begin());
} }
if(guessImuAcceleration_)
{
imuMotionPredictor_.addImu(data.stamp(), data.imu()); imuMotionPredictor_.addImu(data.stamp(), data.imu());
} }
}
else else
{ {
UWARN("Received IMU doesn't have orientation set! It is ignored."); UWARN("Received IMU doesn't have orientation set! It is ignored.");
@@ -661,10 +656,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
orientation.r11(), orientation.r12(), orientation.r13(), guess.x(), orientation.r11(), orientation.r12(), orientation.r13(), guess.x(),
orientation.r21(), orientation.r22(), orientation.r23(), guess.y(), orientation.r21(), orientation.r22(), orientation.r23(), guess.y(),
orientation.r31(), orientation.r32(), orientation.r33(), guess.z()); orientation.r31(), orientation.r32(), orientation.r33(), guess.z());
if(guessFromMotion_ && guessImuAcceleration_ && imuMotionPredictor_.hasPose()) if(guessFromMotion_ && guessSmoothingDelay_ > 0.0f && imuMotionPredictor_.hasPose())
{ {
// Translation (and orientation) predicted from the previous pose with the // Translation (and orientation) predicted from the previous pose with the
// IMU acceleration, instead of a constant velocity. // IMU acceleration, instead of a constant velocity: the velocity is the one
// over the smoothing delay, carried to the previous frame with the IMU.
Transform predicted = imuMotionPredictor_.predict(data.stamp()); Transform predicted = imuMotionPredictor_.predict(data.stamp());
if(!predicted.isNull()) if(!predicted.isNull())
{ {
@@ -688,15 +684,48 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
UTimer time; UTimer time;
// Deskewing lidar // Deskewing lidar, if the scan has a time spread (not already deskewed: deskewing zeroes
if( _deskewing && // the time channel)
const bool scanHasTimeSpread =
!data.laserScanRaw().empty() && !data.laserScanRaw().empty() &&
data.laserScanRaw().hasTime() && data.laserScanRaw().hasTime() &&
data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] !=
data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
if( _deskewing &&
scanHasTimeSpread &&
!imus_.empty() &&
!imuMotionPredictor_.predict(data.stamp()).isNull())
{
UDEBUG("Deskewing with IMU begin");
// Every point's pose predicted with the IMU since the previous frame: orientation
// from the IMU, translation from the velocity (carried with the IMU acceleration
// with a smoothing delay). Before the first pose, only the orientation.
const Transform referenceInverse = imuMotionPredictor_.predict(data.stamp()).inverse();
auto motion = [&](double stamp)
{
Transform pose = imuMotionPredictor_.predict(stamp);
if(pose.isNull())
{
return pose;
}
pose = referenceInverse * pose;
return _force3DoF?pose.to3DoF():pose;
};
LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), motion);
if(!scanDeskewed.isEmpty())
{
data.setLaserScan(scanDeskewed);
}
info->timeDeskewing = time.ticks();
UDEBUG("Deskewing end");
}
else if( _deskewing &&
scanHasTimeSpread &&
dt > 0 && dt > 0 &&
!guess.isNull()) !guess.isNull())
{ {
UDEBUG("Deskewing begin"); UDEBUG("Deskewing begin");
// Recompute velocity // Constant velocity
float vx,vy,vz, vroll,vpitch,vyaw; float vx,vy,vz, vroll,vpitch,vyaw;
guess.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw); guess.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
@@ -708,38 +737,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
vpitch /= dt; vpitch /= dt;
vyaw /= dt; vyaw /= dt;
if(!imus_.empty())
{
float scanTime =
data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] -
data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
// replace orientation velocity based on IMU (if available)
Transform imuFirstScan = Transform::getTransform(imus_,
data.stamp() +
data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()]);
Transform imuLastScan = Transform::getTransform(imus_,
data.stamp() +
data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()]);
if(!imuFirstScan.isNull() && !imuLastScan.isNull())
{
Transform orientation = imuFirstScan.inverse() * imuLastScan;
orientation.getEulerAngles(vroll, vpitch, vyaw);
if(_force3DoF)
{
vroll=0;
vpitch=0;
vyaw /= scanTime;
}
else
{
vroll /= scanTime;
vpitch /= scanTime;
vyaw /= scanTime;
}
}
}
Transform velocity(vx,vy,vz,vroll,vpitch,vyaw); Transform velocity(vx,vy,vz,vroll,vpitch,vyaw);
LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity); LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity);
if(!scanDeskewed.isEmpty()) if(!scanDeskewed.isEmpty())
@@ -902,7 +899,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
} }
} }
if(t.isNull() && guessImuAcceleration_) if(t.isNull())
{ {
// Lost: no velocity can be estimated across the reset that follows // Lost: no velocity can be estimated across the reset that follows
imuMotionPredictor_.addPose(data.stamp(), Transform()); imuMotionPredictor_.addPose(data.stamp(), Transform());
@@ -1063,11 +1060,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
velocityGuess_.setNull(); velocityGuess_.setNull();
} }
if(guessImuAcceleration_)
{ {
const Transform newPose = _pose * t; const Transform newPose = _pose * t;
imuMotionPredictor_.addPose(data.stamp(), newPose); imuMotionPredictor_.addPose(data.stamp(), newPose);
if(!velocityGuess_.isNull() && _filteringStrategy != 1 && particleFilters_.empty()) if(guessSmoothingDelay_ > 0.0f && !imus_.empty() &&
!velocityGuess_.isNull() && _filteringStrategy != 1 && particleFilters_.empty())
{ {
// The translational velocity over the smoothing delay, carried to this // The translational velocity over the smoothing delay, carried to this
// frame with the IMU acceleration (see ImuMotionPredictor), in this frame. // frame with the IMU acceleration (see ImuMotionPredictor), in this frame.
corelib/src/OdometryThread.cpp
+15 -2
View File
@@ -34,6 +34,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h" #include "rtabmap/utilite/UTimer.h"
#include <algorithm>
namespace rtabmap { namespace rtabmap {
OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize) : OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize) :
@@ -236,13 +238,24 @@ bool OdometryThread::getData(SensorEvent & event)
{ {
if(!_dataBuffer.empty()) if(!_dataBuffer.empty())
{ {
// Send IMU up to stamp greater than image (OpenVINS needs this). // Send IMU up to stamp greater than image (OpenVINS needs this). For a lidar
// scan with a time channel, up to the end of its sweep: deskewing predicts the
// pose of every point with the IMU (approaches processing the IMU themselves
// get it as before).
double imuUntil = _dataBuffer.front().data().stamp();
const LaserScan & scan = _dataBuffer.front().data().laserScanRaw();
if(!_odometry->canProcessAsyncIMU() && !scan.isEmpty() && scan.hasTime())
{
imuUntil += std::max(0.0f, std::max(
scan.data().ptr<float>(0, 0)[scan.getTimeOffset()],
scan.data().ptr<float>(0, scan.size()-1)[scan.getTimeOffset()]));
}
while(!_imuBuffer.empty()) while(!_imuBuffer.empty())
{ {
_odometry->process(_imuBuffer.front()); _odometry->process(_imuBuffer.front());
double stamp =_imuBuffer.front().stamp(); double stamp =_imuBuffer.front().stamp();
_imuBuffer.pop_front(); _imuBuffer.pop_front();
if(stamp > _dataBuffer.front().data().stamp()) { if(stamp > imuUntil) {
break; break;
} }
} }
corelib/src/util3d.cpp
+58 -22
View File
@@ -3822,17 +3822,12 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr loadCloud(
return util3d::transformPointCloud(cloud, transform); return util3d::transformPointCloud(cloud, transform);
} }
LaserScan deskew( static LaserScan deskewImpl(
const LaserScan & input, const LaserScan & input,
double inputStamp, double inputStamp,
const rtabmap::Transform & velocity) const std::function<rtabmap::Transform(double)> & motion,
bool slerp)
{ {
if(velocity.isNull())
{
UERROR("velocity should be valid!");
return LaserScan();
}
if(!input.hasTime()) if(!input.hasTime())
{ {
UERROR("input scan doesn't have a \"time\" channel! Supported formats: \"%s\", \"%s\".", UERROR("input scan doesn't have a \"time\" channel! Supported formats: \"%s\", \"%s\".",
@@ -3861,20 +3856,14 @@ LaserScan deskew(
return LaserScan(); return LaserScan();
} }
// With slerp, the poses of the base frame at the first and last stamps (relative to
// the base frame at inputStamp), interpolated in between
rtabmap::Transform firstPose; rtabmap::Transform firstPose;
rtabmap::Transform lastPose; rtabmap::Transform lastPose;
if(slerp)
float vx,vy,vz, vroll,vpitch,vyaw; {
velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw); firstPose = motion(firstStamp);
lastPose = motion(lastStamp);
// 1- The pose of base frame in odom frame at first stamp
// 2- The pose of base frame in odom frame at last stamp
double dt1 = firstStamp - inputStamp;
double dt2 = lastStamp - inputStamp;
firstPose = rtabmap::Transform(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
lastPose = rtabmap::Transform(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2);
if(firstPose.isNull()) if(firstPose.isNull())
{ {
UERROR("Could not get transform between stamps %f and %f!", UERROR("Could not get transform between stamps %f and %f!",
@@ -3889,6 +3878,7 @@ LaserScan deskew(
inputStamp); inputStamp);
return LaserScan(); return LaserScan();
} }
}
double stamp; double stamp;
UTimer processingTime; UTimer processingTime;
@@ -3918,7 +3908,12 @@ LaserScan deskew(
{ {
const float * inputPtr = input.data().ptr<float>(0, u); const float * inputPtr = input.data().ptr<float>(0, u);
stamp = inputStamp + inputPtr[offsetTime]; stamp = inputStamp + inputPtr[offsetTime];
rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose); rtabmap::Transform transform = slerp?firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose):motion(stamp);
if(transform.isNull())
{
UERROR("Could not get transform between stamps %f and %f!", stamp, inputStamp);
return LaserScan();
}
for(int v=0; v<input.data().rows; ++v) for(int v=0; v<input.data().rows; ++v)
{ {
@@ -3960,7 +3955,12 @@ LaserScan deskew(
{ {
const float * inputPtr = input.data().ptr<float>(v, 0); const float * inputPtr = input.data().ptr<float>(v, 0);
stamp = inputStamp + inputPtr[offsetTime]; stamp = inputStamp + inputPtr[offsetTime];
rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose); rtabmap::Transform transform = slerp?firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose):motion(stamp);
if(transform.isNull())
{
UERROR("Could not get transform between stamps %f and %f!", stamp, inputStamp);
return LaserScan();
}
for(int u=0; u<input.data().cols; ++u) for(int u=0; u<input.data().cols; ++u)
{ {
@@ -3996,6 +3996,42 @@ LaserScan deskew(
return LaserScan(output, input.maxPoints(), input.rangeMax(), outputFormat, input.localTransform()); return LaserScan(output, input.maxPoints(), input.rangeMax(), outputFormat, input.localTransform());
} }
LaserScan deskew(
const LaserScan & input,
double inputStamp,
const rtabmap::Transform & velocity)
{
if(velocity.isNull())
{
UERROR("velocity should be valid!");
return LaserScan();
}
float vx,vy,vz, vroll,vpitch,vyaw;
velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
// The pose of the base frame at a stamp relative to the one at inputStamp, with a
// constant velocity: computed at the first and last stamps, interpolated in between
auto motion = [&](double stamp)
{
const double dt = stamp - inputStamp;
return rtabmap::Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
};
return deskewImpl(input, inputStamp, motion, true);
}
LaserScan deskew(
const LaserScan & input,
double inputStamp,
const std::function<rtabmap::Transform(double stamp)> & motion,
bool slerp)
{
if(!motion)
{
UERROR("motion should be set!");
return LaserScan();
}
return deskewImpl(input, inputStamp, motion, slerp);
}
} }
corelib/test/test_imumotionpredictor.cpp
+15
View File
@@ -352,3 +352,18 @@ TEST(ImuMotionPredictor, holds_the_acceleration_at_the_pose_until_a_newer_sample
} }
EXPECT_NEAR(reference.predict(0.3).x(), incremental.predict(0.3).x(), 1e-9); EXPECT_NEAR(reference.predict(0.3).x(), incremental.predict(0.3).x(), 1e-9);
} }
TEST(ImuMotionPredictor, ignores_the_acceleration_without_a_velocity_window)
{
// From rest with a constant 2 m/s^2: with a window of 0, the velocity is the one of the
// last interval and is kept constant, as without IMU.
const double a = 2.0;
ImuMotionPredictor predictor(1.0, 0.0);
addSamples(predictor, -0.05, 0.3,
[](double) { return Eigen::Quaterniond::Identity(); },
[&](double t) { return Eigen::Vector3d(t < 0.0 ? 0.0 : a, 0, 0); });
predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), 0));
predictor.addPose(0.1, pose(Eigen::Vector3d(0.5*a*0.01, 0, 0), 0));
EXPECT_NEAR(predictor.velocity().x(), 0.5*a*0.1, 1e-6);
EXPECT_NEAR(predictor.predict(0.2).x(), 0.5*a*0.01 + 0.5*a*0.1*0.1, 1e-6);
}
corelib/test/test_util3d.cpp
+38
View File
@@ -2114,3 +2114,41 @@ TEST(Util3dTest, DeskewValidScan) {
// empty scan // empty scan
EXPECT_TRUE(util3d::deskew(LaserScan(), inputStamp, velocity).empty()); EXPECT_TRUE(util3d::deskew(LaserScan(), inputStamp, velocity).empty());
} }
TEST(Util3dTest, DeskewWithMotion) {
// Three points measured at y=0, 1 s before, at and 1 s after the scan stamp, while the
// base moves along y as (t-stamp)^2: a motion no constant velocity describes.
cv::Mat data = cv::Mat::zeros(1, 3, CV_32FC(5));
float * dataPtr = (float*)data.data;
dataPtr[0] = 1; dataPtr[4] = -1;
dataPtr[5] = 1; dataPtr[9] = 0;
dataPtr[10] = 1; dataPtr[14] = 1;
LaserScan scan(data, 3, 10.0f, LaserScan::kXYZIT);
const double inputStamp = 1000.0;
std::vector<double> stamps;
auto motion = [&](double stamp) {
stamps.push_back(stamp);
const double dt = stamp - inputStamp;
return Transform(0.0, dt*dt, 0.0, 0.0, 0.0, 0.0);
};
// Asked for every point time, each point is moved by its own pose
LaserScan result = util3d::deskew(scan, inputStamp, motion);
ASSERT_EQ(result.size(), 3);
EXPECT_EQ(stamps.size(), 3u);
EXPECT_FLOAT_EQ(result.field(0, 1), 1.0f);
EXPECT_FLOAT_EQ(result.field(1, 1), 0.0f);
EXPECT_FLOAT_EQ(result.field(2, 1), 1.0f);
// With slerp, only the ends are asked for, the middle point is interpolated between them
stamps.clear();
result = util3d::deskew(scan, inputStamp, motion, true);
ASSERT_EQ(result.size(), 3);
EXPECT_EQ(stamps.size(), 2u);
EXPECT_FLOAT_EQ(result.field(1, 1), 1.0f);
// A failing motion, or none
EXPECT_TRUE(util3d::deskew(scan, inputStamp, [](double) { return Transform(); }).empty());
EXPECT_TRUE(util3d::deskew(scan, inputStamp, std::function<Transform(double)>()).empty());
}
guilib/src/PreferencesDialog.cpp
-1
View File
@@ -1504,7 +1504,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->odom_flow_scanKeyframeThr->setObjectName(Parameters::kOdomScanKeyFrameThr().c_str()); _ui->odom_flow_scanKeyframeThr->setObjectName(Parameters::kOdomScanKeyFrameThr().c_str());
_ui->odom_flow_guessMotion->setObjectName(Parameters::kOdomGuessMotion().c_str()); _ui->odom_flow_guessMotion->setObjectName(Parameters::kOdomGuessMotion().c_str());
_ui->odom_guess_smoothing_delay->setObjectName(Parameters::kOdomGuessSmoothingDelay().c_str()); _ui->odom_guess_smoothing_delay->setObjectName(Parameters::kOdomGuessSmoothingDelay().c_str());
_ui->odom_guess_imu_acceleration->setObjectName(Parameters::kOdomGuessImuAcceleration().c_str());
_ui->odom_imu_gravity->setObjectName(Parameters::kOdomImuGravity().c_str()); _ui->odom_imu_gravity->setObjectName(Parameters::kOdomImuGravity().c_str());
_ui->odom_imageDecimation->setObjectName(Parameters::kOdomImageDecimation().c_str()); _ui->odom_imageDecimation->setObjectName(Parameters::kOdomImageDecimation().c_str());
_ui->odom_alignWithGround->setObjectName(Parameters::kOdomAlignWithGround().c_str()); _ui->odom_alignWithGround->setObjectName(Parameters::kOdomAlignWithGround().c_str());
guilib/src/ui/preferencesDialog.ui
+23 -43
View File
@@ -16427,7 +16427,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
</property> </property>
</widget> </widget>
</item> </item>
<item row="16" column="1"> <item row="15" column="1">
<widget class="QLabel" name="label_232"> <widget class="QLabel" name="label_232">
<property name="text"> <property name="text">
<string>Data buffer size (0 means inf).</string> <string>Data buffer size (0 means inf).</string>
@@ -16570,7 +16570,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
</property> </property>
</widget> </widget>
</item> </item>
<item row="14" column="0"> <item row="13" column="0">
<widget class="QDoubleSpinBox" name="odom_flow_keyframeThr"> <widget class="QDoubleSpinBox" name="odom_flow_keyframeThr">
<property name="maximum"> <property name="maximum">
<double>1.000000000000000</double> <double>1.000000000000000</double>
@@ -16590,7 +16590,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
</property> </property>
</widget> </widget>
</item> </item>
<item row="13" column="0"> <item row="12" column="0">
<widget class="QSpinBox" name="odom_VisKeyFrameThr"> <widget class="QSpinBox" name="odom_VisKeyFrameThr">
<property name="maximum"> <property name="maximum">
<number>9999</number> <number>9999</number>
@@ -16600,7 +16600,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
</property> </property>
</widget> </widget>
</item> </item>
<item row="15" column="1"> <item row="14" column="1">
<widget class="QLabel" name="label_246"> <widget class="QLabel" name="label_246">
<property name="text"> <property name="text">
<string>[Geometry] Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame.</string> <string>[Geometry] Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame.</string>
@@ -16613,7 +16613,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
</property> </property>
</widget> </widget>
</item> </item>
<item row="12" column="1"> <item row="11" column="1">
<widget class="QLabel" name="label_248"> <widget class="QLabel" name="label_248">
<property name="text"> <property name="text">
<string>Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If Visual Registration -&gt; Visual Feature -&gt; Depth as Mask is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.</string> <string>Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If Visual Registration -&gt; Visual Feature -&gt; Depth as Mask is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.</string>
@@ -16709,7 +16709,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
</property> </property>
</widget> </widget>
</item> </item>
<item row="17" column="0"> <item row="16" column="0">
<widget class="QPushButton" name="pushButton_testOdometry"> <widget class="QPushButton" name="pushButton_testOdometry">
<property name="text"> <property name="text">
<string>Test odometry</string> <string>Test odometry</string>
@@ -16729,14 +16729,14 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
</property> </property>
</widget> </widget>
</item> </item>
<item row="16" column="0"> <item row="15" column="0">
<widget class="QSpinBox" name="odom_dataBufferSize"> <widget class="QSpinBox" name="odom_dataBufferSize">
<property name="maximum"> <property name="maximum">
<number>999999</number> <number>999999</number>
</property> </property>
</widget> </widget>
</item> </item>
<item row="14" column="1"> <item row="13" column="1">
<widget class="QLabel" name="label_196"> <widget class="QLabel" name="label_196">
<property name="text"> <property name="text">
<string>[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.</string> <string>[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.</string>
@@ -16749,7 +16749,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
</property> </property>
</widget> </widget>
</item> </item>
<item row="12" column="0"> <item row="11" column="0">
<widget class="QSpinBox" name="odom_imageDecimation"> <widget class="QSpinBox" name="odom_imageDecimation">
<property name="minimum"> <property name="minimum">
<number>1</number> <number>1</number>
@@ -16778,7 +16778,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
</property> </property>
</widget> </widget>
</item> </item>
<item row="15" column="0"> <item row="14" column="0">
<widget class="QDoubleSpinBox" name="odom_flow_scanKeyframeThr"> <widget class="QDoubleSpinBox" name="odom_flow_scanKeyframeThr">
<property name="maximum"> <property name="maximum">
<double>1.000000000000000</double> <double>1.000000000000000</double>
@@ -16794,7 +16794,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
<item row="8" column="1"> <item row="8" column="1">
<widget class="QLabel" name="label_520"> <widget class="QLabel" name="label_520">
<property name="text"> <property name="text">
<string>Guess smoothing delay (s). Estimated velocity is averaged based on last transforms up to this maximum delay. This can help to get smoother velocity prediction. Last velocity computed is used directly if a filtering strategy is set or the delay is below the odometry rate. Recommended (~0.5 s) with the IMU acceleration below: the velocity is then corrected by the IMU acceleration, so that it is the velocity at the last frame rather than a delayed average. Without IMU, the average is delayed by half this delay: it can make sense for platforms with inertia (e.g., cars at high speed, where one bad frame would otherwise change the predicted velocity a lot), for high frame rates, or when the velocity is used for lidar deskewing, where its noise would feed back into the next poses.</string> <string>Guess smoothing delay (s). Estimated velocity is averaged based on last transforms up to this maximum delay. This can help to get smoother velocity prediction. Last velocity computed is used directly if a filtering strategy is set or the delay is below the odometry rate. With an IMU giving orientation and linear acceleration, a delay &gt; 0 also enables the IMU acceleration: the velocity is then corrected by the acceleration measured since, so that it is the velocity at the last frame rather than a delayed average, and the motion guess and lidar deskewing integrate the acceleration from it. Recommended (~0.5 s) with an IMU; with 0, the IMU only gives the orientation. Without IMU, the average is delayed by half this delay: it can make sense for platforms with inertia (e.g., cars at high speed, where one bad frame would otherwise change the predicted velocity a lot), for high frame rates, or when the velocity is used for lidar deskewing, where its noise would feed back into the next poses.</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>
@@ -16831,7 +16831,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
</property> </property>
</widget> </widget>
</item> </item>
<item row="13" column="1"> <item row="12" column="1">
<widget class="QLabel" name="label_354"> <widget class="QLabel" name="label_354">
<property name="text"> <property name="text">
<string>[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.</string> <string>[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.</string>
@@ -16844,37 +16844,10 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
</property> </property>
</widget> </widget>
</item> </item>
<item row="11" column="1"> <item row="10" column="1">
<widget class="QLabel" name="label_7461"> <widget class="QLabel" name="label_7461">
<property name="text"> <property name="text">
<string>Lidar deskewing. If input lidar has time channel, it will be deskewed with a constant motion model (with IMU orientation and/or guess if provided).</string> <string>Lidar deskewing. If input lidar has time channel, it will be deskewed. With an IMU, the pose of every point is predicted from the previous frame: orientation from the IMU, translation from the velocity (with the IMU acceleration if the guess smoothing delay is &gt; 0). Without IMU, with a constant motion model (or the guess if provided).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QCheckBox" name="odom_lidar_deskewing">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QCheckBox" name="odom_guess_imu_acceleration">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QLabel" name="label_odom_guess_imu_acceleration">
<property name="text">
<string>With an IMU giving orientation and linear acceleration, predict the translation of the motion guess by integrating the IMU acceleration from the velocity at the previous frame, instead of with a constant velocity. That velocity is the average over the guess smoothing delay above (set it to about 0.5 s), carried to the previous frame with the acceleration measured since: it is not delayed like the average alone. Ignored by odometry approaches processing the IMU themselves.</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>
@@ -16885,6 +16858,13 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
</widget> </widget>
</item> </item>
<item row="10" column="0"> <item row="10" column="0">
<widget class="QCheckBox" name="odom_lidar_deskewing">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QDoubleSpinBox" name="odom_imu_gravity"> <widget class="QDoubleSpinBox" name="odom_imu_gravity">
<property name="suffix"> <property name="suffix">
<string> m/s²</string> <string> m/s²</string>
@@ -16903,10 +16883,10 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
</property> </property>
</widget> </widget>
</item> </item>
<item row="10" column="1"> <item row="9" column="1">
<widget class="QLabel" name="label_odom_imu_gravity"> <widget class="QLabel" name="label_odom_imu_gravity">
<property name="text"> <property name="text">
<string>Gravity magnitude removed from the IMU linear acceleration with the IMU acceleration above. Standard gravity by default. Set it to what the accelerometer reads at rest to compensate for its scale error, or for another gravity than Earth's.</string> <string>Gravity magnitude removed from the IMU linear acceleration (used with a guess smoothing delay &gt; 0 above). Standard gravity by default. Set it to what the accelerometer reads at rest to compensate for its scale error, or for another gravity than Earth's.</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>