mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-11 20:39:52 +08:00
refactored odom's imu init orientation
This commit is contained in:
matlabbe
+64
-41
@@ -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<double, rtabmap::Transform> 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<double, rtabmap::Transform> 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();
|
||||
|
||||
@@ -11,6 +11,10 @@
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/imgcodecs.hpp>
|
||||
#include <memory>
|
||||
#include <rtabmap/core/IMU.h>
|
||||
#include <rtabmap/utilite/UStl.h>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <string>
|
||||
|
||||
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<kColumns; ++u)
|
||||
{
|
||||
const double dt = kSweep * double(u) / double(kColumns-1);
|
||||
const Transform pose = trajectory.pose(stamp + dt);
|
||||
const Eigen::Matrix3d rotation = pose.toEigen3d().linear();
|
||||
const Eigen::Vector3d origin(pose.x(), pose.y(), pose.z());
|
||||
const double azimuth = 2.0*M_PI*double(u)/double(kColumns);
|
||||
for(int v=0; v<kRings; ++v)
|
||||
{
|
||||
const double elevation = (-15.0 + 30.0*double(v)/double(kRings-1)) * M_PI / 180.0;
|
||||
const Eigen::Vector3d direction(std::cos(elevation)*std::cos(azimuth), std::cos(elevation)*std::sin(azimuth), std::sin(elevation));
|
||||
const Eigen::Vector3d world = rotation * direction;
|
||||
// Distance to the wall the ray hits
|
||||
double range = std::numeric_limits<double>::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<float>(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<scan.size(); ++i)
|
||||
{
|
||||
const float * p = scan.data().ptr<float>(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<double>::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(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(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<Odometry> 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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user