mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-11 20:39:52 +08:00
simplified parameters
This commit is contained in:
matlabbe
@@ -66,16 +66,20 @@ class RTABMAP_CORE_EXPORT ImuMotionPredictor
|
||||
{
|
||||
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).
|
||||
*
|
||||
* @param maxPoseInterval odometry poses older (s) than this are not used to estimate
|
||||
* the velocity, which is null without one
|
||||
* @param velocityWindow the velocity is estimated from the displacement since the
|
||||
* newest pose at least this old (s). Over a single frame
|
||||
* interval, the noise of the odometry poses would be of the order
|
||||
* of the velocity itself; with the acceleration, a longer window
|
||||
* still gives the velocity at the last pose, not an average.
|
||||
* newest pose at least this old (s), corrected by the
|
||||
* acceleration measured since, so that it is the velocity at the
|
||||
* last pose, not an average. Over a single frame interval, the
|
||||
* 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
|
||||
* given to addImu(), standard gravity by default
|
||||
*
|
||||
|
||||
@@ -160,7 +160,6 @@ private:
|
||||
bool _force3DoF;
|
||||
bool _holonomic;
|
||||
bool guessFromMotion_;
|
||||
bool guessImuAcceleration_;
|
||||
float guessSmoothingDelay_;
|
||||
int _filteringStrategy;
|
||||
int _particleSize;
|
||||
@@ -191,7 +190,7 @@ private:
|
||||
std::vector<StereoCameraModel> stereoModels_;
|
||||
std::vector<CameraModel> models_;
|
||||
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 */
|
||||
|
||||
@@ -512,15 +512,14 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
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, 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 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. 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, 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, 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, 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, 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, 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
|
||||
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.");
|
||||
|
||||
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#ifndef UTIL3D_H_
|
||||
#define UTIL3D_H_
|
||||
|
||||
#include <functional>
|
||||
#include "rtabmap/core/rtabmap_core_export.h"
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
@@ -1295,6 +1296,27 @@ LaserScan RTABMAP_CORE_EXPORT deskew(
|
||||
double inputStamp,
|
||||
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 rtabmap
|
||||
|
||||
|
||||
@@ -133,7 +133,8 @@ void ImuMotionPredictor::addPose(double stamp, const rtabmap::Transform & pose)
|
||||
pose.y() - reference->second.y(),
|
||||
pose.z() - reference->second.z());
|
||||
|
||||
if(!samples_.empty() &&
|
||||
if(velocityWindow_ > 0.0 &&
|
||||
!samples_.empty() &&
|
||||
samples_.begin()->first <= reference->first &&
|
||||
samples_.rbegin()->first >= stamp)
|
||||
{
|
||||
@@ -147,7 +148,7 @@ void ImuMotionPredictor::addPose(double stamp, const rtabmap::Transform & pose)
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -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.
|
||||
Eigen::Vector3d position(pose_.x(), pose_.y(), pose_.z()); // p0
|
||||
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
|
||||
// the part from the last sample ti <= t is integrated here, with dt = t-ti:
|
||||
|
||||
+46
-49
@@ -134,7 +134,6 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
|
||||
_force3DoF(Parameters::defaultRegForce3DoF()),
|
||||
_holonomic(Parameters::defaultOdomHolonomic()),
|
||||
guessFromMotion_(Parameters::defaultOdomGuessMotion()),
|
||||
guessImuAcceleration_(Parameters::defaultOdomGuessImuAcceleration()),
|
||||
guessSmoothingDelay_(Parameters::defaultOdomGuessSmoothingDelay()),
|
||||
_filteringStrategy(Parameters::defaultOdomFilteringStrategy()),
|
||||
_particleSize(Parameters::defaultOdomParticleSize()),
|
||||
@@ -161,13 +160,12 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
|
||||
Parameters::parse(parameters, Parameters::kRegForce3DoF(), _force3DoF);
|
||||
Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
|
||||
Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
|
||||
Parameters::parse(parameters, Parameters::kOdomGuessImuAcceleration(), guessImuAcceleration_);
|
||||
Parameters::parse(parameters, Parameters::kOdomGuessSmoothingDelay(), guessSmoothingDelay_);
|
||||
if(guessImuAcceleration_)
|
||||
{
|
||||
float imuGravity = Parameters::defaultOdomImuGravity();
|
||||
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);
|
||||
}
|
||||
Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
|
||||
@@ -351,10 +349,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
|
||||
imus_.erase(imus_.begin());
|
||||
}
|
||||
|
||||
if(guessImuAcceleration_)
|
||||
{
|
||||
imuMotionPredictor_.addImu(data.stamp(), data.imu());
|
||||
}
|
||||
imuMotionPredictor_.addImu(data.stamp(), data.imu());
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -661,10 +656,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
|
||||
orientation.r11(), orientation.r12(), orientation.r13(), guess.x(),
|
||||
orientation.r21(), orientation.r22(), orientation.r23(), guess.y(),
|
||||
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
|
||||
// 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());
|
||||
if(!predicted.isNull())
|
||||
{
|
||||
@@ -688,15 +684,48 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
|
||||
|
||||
UTimer time;
|
||||
|
||||
// Deskewing lidar
|
||||
// Deskewing lidar, if the scan has a time spread (not already deskewed: deskewing zeroes
|
||||
// the time channel)
|
||||
const bool scanHasTimeSpread =
|
||||
!data.laserScanRaw().empty() &&
|
||||
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 &&
|
||||
!data.laserScanRaw().empty() &&
|
||||
data.laserScanRaw().hasTime() &&
|
||||
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 &&
|
||||
!guess.isNull())
|
||||
{
|
||||
UDEBUG("Deskewing begin");
|
||||
// Recompute velocity
|
||||
// Constant velocity
|
||||
float 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;
|
||||
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);
|
||||
LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity);
|
||||
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
|
||||
imuMotionPredictor_.addPose(data.stamp(), Transform());
|
||||
@@ -1063,11 +1060,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
|
||||
velocityGuess_.setNull();
|
||||
}
|
||||
|
||||
if(guessImuAcceleration_)
|
||||
{
|
||||
const Transform newPose = _pose * t;
|
||||
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
|
||||
// frame with the IMU acceleration (see ImuMotionPredictor), in this frame.
|
||||
|
||||
@@ -34,6 +34,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "rtabmap/utilite/ULogger.h"
|
||||
#include "rtabmap/utilite/UTimer.h"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize) :
|
||||
@@ -236,13 +238,24 @@ bool OdometryThread::getData(SensorEvent & event)
|
||||
{
|
||||
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())
|
||||
{
|
||||
_odometry->process(_imuBuffer.front());
|
||||
double stamp =_imuBuffer.front().stamp();
|
||||
_imuBuffer.pop_front();
|
||||
if(stamp > _dataBuffer.front().data().stamp()) {
|
||||
if(stamp > imuUntil) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
+70
-34
@@ -3822,17 +3822,12 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr loadCloud(
|
||||
return util3d::transformPointCloud(cloud, transform);
|
||||
}
|
||||
|
||||
LaserScan deskew(
|
||||
static LaserScan deskewImpl(
|
||||
const LaserScan & input,
|
||||
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())
|
||||
{
|
||||
UERROR("input scan doesn't have a \"time\" channel! Supported formats: \"%s\", \"%s\".",
|
||||
@@ -3861,33 +3856,28 @@ LaserScan deskew(
|
||||
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 lastPose;
|
||||
|
||||
float vx,vy,vz, vroll,vpitch,vyaw;
|
||||
velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
|
||||
|
||||
// 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(slerp)
|
||||
{
|
||||
UERROR("Could not get transform between stamps %f and %f!",
|
||||
firstStamp,
|
||||
inputStamp);
|
||||
return LaserScan();
|
||||
}
|
||||
if(lastPose.isNull())
|
||||
{
|
||||
UERROR("Could not get transform between stamps %f and %f!",
|
||||
lastStamp,
|
||||
inputStamp);
|
||||
return LaserScan();
|
||||
firstPose = motion(firstStamp);
|
||||
lastPose = motion(lastStamp);
|
||||
if(firstPose.isNull())
|
||||
{
|
||||
UERROR("Could not get transform between stamps %f and %f!",
|
||||
firstStamp,
|
||||
inputStamp);
|
||||
return LaserScan();
|
||||
}
|
||||
if(lastPose.isNull())
|
||||
{
|
||||
UERROR("Could not get transform between stamps %f and %f!",
|
||||
lastStamp,
|
||||
inputStamp);
|
||||
return LaserScan();
|
||||
}
|
||||
}
|
||||
|
||||
double stamp;
|
||||
@@ -3918,7 +3908,12 @@ LaserScan deskew(
|
||||
{
|
||||
const float * inputPtr = input.data().ptr<float>(0, u);
|
||||
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)
|
||||
{
|
||||
@@ -3960,7 +3955,12 @@ LaserScan deskew(
|
||||
{
|
||||
const float * inputPtr = input.data().ptr<float>(v, 0);
|
||||
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)
|
||||
{
|
||||
@@ -3996,6 +3996,42 @@ LaserScan deskew(
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
@@ -2114,3 +2114,41 @@ TEST(Util3dTest, DeskewValidScan) {
|
||||
// empty scan
|
||||
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());
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user