From 9e27446236b4fff21a9dc77a2b9e34c2c8a95b94 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 10 Oct 2026 22:12:50 -0700 Subject: [PATCH] refactored odom's imu init orientation --- corelib/src/Odometry.cpp | 105 ++++++++++------- corelib/test/test_odometry.cpp | 209 +++++++++++++++++++++++++++++++++ 2 files changed, 273 insertions(+), 41 deletions(-) diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index ba02ee78..338f58ce 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -320,40 +320,47 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet { UASSERT_MSG(data.id() >= 0, uFormat("Input data should have ID greater or equal than 0 (id=%d)!", data.id()).c_str()); - // cache imu data - if(!data.imu().empty() && !this->canProcessAsyncIMU()) + if(!data.imu().empty()) { - if(!(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0)) + if(!this->canProcessAsyncIMU()) { - Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]); - // orientation includes roll and pitch but not yaw in local transform - Transform imuT = Transform(data.imu().localTransform().x(),data.imu().localTransform().y(),data.imu().localTransform().z(), 0,0,data.imu().localTransform().theta()) * - orientation* - data.imu().localTransform().rotation().inverse(); - - if( this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f && - this->framesProcessed() == 0) + // cache imu data + if(!(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0)) { - Eigen::Quaterniond imuQuat = imuT.getQuaterniond(); - Transform previous = this->getPose(); - Transform newFramePose = Transform(previous.x(), previous.y(), previous.z(), imuQuat.x(), imuQuat.y(), imuQuat.z(), imuQuat.w()); - UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), newFramePose.prettyPrint().c_str()); - std::map imus = imus_; - this->reset(newFramePose); - imus_ = imus; - } + Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]); + // orientation includes roll and pitch but not yaw in local transform + Transform imuT = Transform(data.imu().localTransform().x(),data.imu().localTransform().y(),data.imu().localTransform().z(), 0,0,data.imu().localTransform().theta()) * + orientation* + data.imu().localTransform().rotation().inverse(); - imus_.insert(std::make_pair(data.stamp(), imuT)); - if(imus_.size() > 1000) + imus_.insert(std::make_pair(data.stamp(), imuT)); + if(imus_.size() > 1000) + { + imus_.erase(imus_.begin()); + } + + imuMotionPredictor_.addImu(data.stamp(), data.imu()); + } + else { - imus_.erase(imus_.begin()); + UWARN("Received IMU doesn't have orientation set! It is ignored."); } - - imuMotionPredictor_.addImu(data.stamp(), data.imu()); } - else + + // IMU-only update: nothing more to do once the IMU is cached, except for approaches + // processing it themselves. A frame that brings its own features carries no image, + // and a frame whose scene was empty carries no feature either, so neither says + // whether there is a frame at all. The calibration does: it is there when a camera + // produced this data. + if(data.imageRaw().empty() && data.imageCompressed().empty() && + data.laserScanRaw().isEmpty() && data.laserScanCompressed().isEmpty() && + data.cameraModels().empty() && data.stereoCameraModels().empty()) { - UWARN("Received IMU doesn't have orientation set! It is ignored."); + if(this->canProcessAsyncIMU()) + { + this->computeTransform(data, Transform(), info); + } + return Transform(); // Return null on IMU-only updates } } @@ -598,12 +605,40 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet } } + // Initial orientation from the IMU at the first frame's own stamp (unless an initial pose + // with a rotation was given). Not from the first IMU sample received, which can be much + // older (e.g., IMU buffered while waiting for the first frame), nor from the newest one, + // which can be after the stamp (lidar deskewing needs IMU up to the end of the sweep). + if(this->framesProcessed() == 0 && !imus_.empty() && + this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f) + { + // Interpolated at the stamp, or the closest sample if the IMU doesn't cover it (e.g., + // the only sample received is just after the frame) + Transform imuAtStamp = Transform::getTransform(imus_, data.stamp()); + if(imuAtStamp.isNull()) + { + imuAtStamp = data.stamp() < imus_.begin()->first?imus_.begin()->second:imus_.rbegin()->second; + } + if(!imuAtStamp.isNull()) + { + const Eigen::Quaterniond q = imuAtStamp.getQuaterniond(); + const Transform previous = this->getPose(); + const Transform initialPose(previous.x(), previous.y(), previous.z(), q.x(), q.y(), q.z(), q.w()); + UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), initialPose.prettyPrint().c_str()); + std::map imus = imus_; + ImuMotionPredictor imuMotionPredictor = imuMotionPredictor_; + this->reset(initialPose); + imus_ = imus; + imuMotionPredictor_ = imuMotionPredictor; + } + } + // KITTI datasets start with stamp=0 double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0; Transform guess = dt>0.0 && guessFromMotion_ && !velocityGuess_.isNull()?Transform::getIdentity():Transform(); if(!(dt>0.0 || (dt == 0.0 && velocityGuess_.isNull()))) { - if(guessFromMotion_ && (!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())) + if(guessFromMotion_) { UERROR("Guess from motion is set but dt is invalid! Odometry is then computed without guess. (dt=%f previous transform=%s)", dt, velocityGuess_.prettyPrint().c_str()); } @@ -678,7 +713,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet UWARN("Could not find imu transform at %f", data.stamp()); } } - else if(!guess.isNull() && (!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())) { + else if(!guess.isNull()) { UDEBUG("Using guess from motion %s", guess.prettyPrint().c_str()); } @@ -859,23 +894,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet } } } - // A frame that brings its own features carries no image, and a frame whose scene was - // empty carries no feature either, so neither says whether there is a frame at all. - // The calibration does: it is there when a camera produced this data. - else if(!data.imageRaw().empty() || - !data.cameraModels().empty() || - !data.stereoCameraModels().empty() || - !data.laserScanRaw().isEmpty() || - (this->canProcessAsyncIMU() && !data.imu().empty())) + else { t = this->computeTransform(data, guess, info); } - if(data.imageRaw().empty() && data.laserScanRaw().isEmpty() && !data.imu().empty()) - { - return Transform(); // Return null on IMU-only updates - } - if(info) { info->timeEstimation = time.ticks(); diff --git a/corelib/test/test_odometry.cpp b/corelib/test/test_odometry.cpp index d1efe483..307974cf 100644 --- a/corelib/test/test_odometry.cpp +++ b/corelib/test/test_odometry.cpp @@ -11,6 +11,10 @@ #include #include #include +#include +#include +#include +#include #include using namespace rtabmap; @@ -584,3 +588,208 @@ TEST(OdometryTest, RefusesAFirstScanTooSmallForTheCorrespondenceRatio) EXPECT_FALSE(unchecked->process(uncheckedData).isNull()) << "a scan of unknown sweep size was refused"; } + +// --------------------------------------------------------------------------- +// Lidar deskewing with an IMU (Odom/Deskewing): a sensor moving in a box-shaped room, +// whose scans are generated point by point from where the sensor was at each point's +// time, as a spinning lidar measures them. +// --------------------------------------------------------------------------- + +namespace { + +const Eigen::Vector3d kRoomMin(-6.0, -4.0, -1.5); +const Eigen::Vector3d kRoomMax(7.0, 5.0, 2.5); +const double kSweep = 0.1; // s, first column to last +const int kRings = 16; +const int kColumns = 512; +const double kGravity = 9.80665; + +/// Pose of the sensor at time t, and its acceleration (world frame). +struct Trajectory +{ + double yaw = 0.0; // rad, heading at t=0 + double yawRate = 0.0; // rad/s + double acceleration = 0.0; // m/s^2 along x, from rest at t=0 + Transform pose(double t) const + { + return Transform(float(0.5*acceleration*t*t), 0, 0, 0, 0, float(yaw + yawRate*t)); + } + Eigen::Vector3d linearAcceleration() const { return Eigen::Vector3d(acceleration, 0, 0); } +}; + +/// The scan of a sweep starting at @p stamp: organized (rings x columns), time on columns. +LaserScan makeSweep(const Trajectory & trajectory, double stamp) +{ + cv::Mat data(kRings, kColumns, CV_32FC(5)); + for(int u=0; u::max(); + for(int i=0; i<3; ++i) + { + if(world[i] > 1e-9) range = std::min(range, (kRoomMax[i] - origin[i]) / world[i]); + else if(world[i] < -1e-9) range = std::min(range, (kRoomMin[i] - origin[i]) / world[i]); + } + float * p = data.ptr(v, u); + p[0] = float(range*direction.x()); + p[1] = float(range*direction.y()); + p[2] = float(range*direction.z()); + p[3] = 1.0f; + p[4] = float(dt); + } + } + return LaserScan(data, kRings*kColumns, 0.0f, LaserScan::kXYZIT, Transform::getIdentity()); +} + +/// What the IMU, at the sensor's origin, measures at time t. +IMU makeImu(const Trajectory & trajectory, double t) +{ + const Eigen::Quaterniond q = trajectory.pose(t).getQuaterniond(); + const Eigen::Vector3d f = q.inverse() * (trajectory.linearAcceleration() + Eigen::Vector3d(0, 0, kGravity)); + return IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat::eye(3,3,CV_64FC1), + cv::Vec3d(0, 0, trajectory.yawRate), cv::Mat::eye(3,3,CV_64FC1), + cv::Vec3d(f.x(), f.y(), f.z()), cv::Mat::eye(3,3,CV_64FC1), + Transform::getIdentity()); +} + +/// RMS distance (m) of the scan's points to the nearest wall, with the sensor at @p pose. +double wallDistance(const LaserScan & scan, const Transform & pose) +{ + double sum = 0.0; + int n = 0; + for(int i=0; i(0, i); + const Transform world = pose * Transform(p[0], p[1], p[2], 0, 0, 0); + const Eigen::Vector3d w(world.x(), world.y(), world.z()); + double d = std::numeric_limits::max(); + for(int k=0; k<3; ++k) + { + d = std::min(d, std::fabs(w[k] - kRoomMin[k])); + d = std::min(d, std::fabs(w[k] - kRoomMax[k])); + } + sum += d*d; + ++n; + } + return n?std::sqrt(sum/n):0.0; +} + +/** + * Feeds @p frames sweeps, one every 0.1 s from t=0.1, with the IMU at 200 Hz up to the end + * of each sweep (as OdometryThread does), and returns the RMS wall distance of the last + * sweep as odometry left it in the frame (deskewed or not). + */ +double deskewedWallDistance(const Trajectory & trajectory, int frames, const ParametersMap & extra) +{ + ParametersMap parameters; + parameters.insert(ParametersPair(Parameters::kRegStrategy(), "1")); + parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0")); + parameters.insert(ParametersPair(Parameters::kIcpPointToPlane(), "true")); + parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneK(), "10")); + parameters.insert(ParametersPair(Parameters::kIcpMaxTranslation(), "0")); + parameters.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), "0.5")); + for(const ParametersPair & p : extra) uInsert(parameters, p); + std::unique_ptr odometry(Odometry::create(parameters)); + + double imuStamp = 0.0; + double distance = -1.0; + for(int i=1; i<=frames; ++i) + { + const double stamp = 0.1*i; + for(; imuStamp <= stamp + kSweep + 0.005; imuStamp += 0.005) + { + SensorData imu(makeImu(trajectory, imuStamp), 0, imuStamp); + odometry->process(imu); + } + SensorData data(makeSweep(trajectory, stamp), cv::Mat(), cv::Mat(), CameraModel(), i, stamp); + OdometryInfo info; + const Transform pose = odometry->process(data, &info); + EXPECT_FALSE(pose.isNull()) << "frame " << i << " not registered"; + distance = wallDistance(data.laserScanRaw(), trajectory.pose(stamp)); + } + return distance; +} + +} // namespace + +TEST(OdometryTest, DeskewsWithTheImuOrientation) +{ + // Turning at 2 rad/s: 11 degrees over a sweep, so the far walls are smeared by about a + // metre. On the first frame, before any pose, only the IMU orientation is used. + Trajectory turning; + turning.yawRate = 2.0; + ParametersMap off; + off.insert(ParametersPair(Parameters::kOdomDeskewing(), "false")); + const double skewed = deskewedWallDistance(turning, 1, off); + const double deskewed = deskewedWallDistance(turning, 1, ParametersMap()); + EXPECT_GT(skewed, 0.1) << "the scan should be smeared without deskewing"; + EXPECT_LT(deskewed, 0.01) << "the points should be back on the walls"; +} + +TEST(OdometryTest, DeskewsWithTheImuAccelerationOverTheSmoothingDelay) +{ + // Accelerating at 2 m/s^2 from rest. With Odom/GuessSmoothingDelay, the velocity is + // carried to each frame with the IMU acceleration, and the prediction integrates it + // over the sweep. At 0, the acceleration is not used: the velocity of the last + // interval lags behind, and the sweep is still skewed. + Trajectory accelerating; + accelerating.acceleration = 2.0; + // Not facing exactly along x: an IMU orientation whose x, y and z are all zero (the + // identity) is read by RTAB-Map's odometry as "not set", and the IMU ignored. + accelerating.yaw = 0.3; + ParametersMap noDelay; + noDelay.insert(ParametersPair(Parameters::kOdomGuessSmoothingDelay(), "0")); + ParametersMap delay; + delay.insert(ParametersPair(Parameters::kOdomGuessSmoothingDelay(), "0.5")); + const double withoutAcceleration = deskewedWallDistance(accelerating, 12, noDelay); + const double withAcceleration = deskewedWallDistance(accelerating, 12, delay); + EXPECT_LT(withAcceleration, 0.005) << "the points should be back on the walls"; + EXPECT_GT(withoutAcceleration, withAcceleration * 3.0); +} + +TEST(OdometryTest, InitialOrientationIsTheImuOrientationAtTheFirstFrame) +{ + // IMU received for 2 s before the first frame, while the sensor turns at 1 rad/s: the + // first frame must start from the orientation at its own stamp, not at the first IMU + // sample (2 rad earlier). + Trajectory turning; + turning.yaw = 0.3; + turning.yawRate = 1.0; + ParametersMap parameters; + parameters.insert(ParametersPair(Parameters::kRegStrategy(), "1")); + parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0")); + std::unique_ptr odometry(Odometry::create(parameters)); + const double stamp = 2.0; + for(double t = 0.0; t <= stamp + kSweep + 0.005; t += 0.005) + { + SensorData imu(makeImu(turning, t), 0, t); + odometry->process(imu); + } + SensorData data(makeSweep(turning, stamp), cv::Mat(), cv::Mat(), CameraModel(), 1, stamp); + OdometryInfo info; + const Transform pose = odometry->process(data, &info); + ASSERT_FALSE(pose.isNull()); + // The IMU was fed up to the end of the sweep (as OdometryThread does), but the orientation + // is interpolated at the frame's stamp + EXPECT_NEAR(pose.theta(), turning.yaw + turning.yawRate*stamp, 0.002); + + // An initial pose given with a rotation is kept + std::unique_ptr given(Odometry::create(parameters)); + given->reset(Transform(0, 0, 0, 0, 0, 1.0f)); + for(double t = 0.0; t <= stamp; t += 0.005) + { + SensorData imu(makeImu(turning, t), 0, t); + given->process(imu); + } + EXPECT_NEAR(given->getPose().theta(), 1.0, 1e-6); +}