i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-12 21:09:50 +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/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.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:
corelib/src/Odometry.cpp
+46 -49
View File
@@ -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.
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/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;
}
}
corelib/src/util3d.cpp
+70 -34
View File
@@ -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);
}
}