mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-13 05:19:51 +08:00
Imu motion predictor (#1791)
* Imu motion predictor * simplified parameters * refactored odom's imu init orientation * fixing sign issue with x86-64-v3 * fixing M_PI not defined in tests * Added auto gravity estimation mode * Fixing crash on windows when uFormat() is to long
This commit is contained in:
matlabbe
@@ -42,8 +42,10 @@ set(corelib_test_sources
|
||||
test_link.cpp #Link.h
|
||||
test_optimizer.cpp #Optimizer.h
|
||||
test_gps.cpp #GPS.h
|
||||
test_parameters.cpp #Parameters.h
|
||||
test_imu.cpp #IMU.h
|
||||
test_imufilter.cpp #IMUFilter.h
|
||||
test_imumotionpredictor.cpp #ImuMotionPredictor.h
|
||||
test_imuthread.cpp #IMUThread.h
|
||||
test_landmark.cpp #Landmark.h
|
||||
test_localgrid.cpp #LocalGrid.h
|
||||
|
||||
@@ -0,0 +1,460 @@
|
||||
// M_PI on MSVC (must come before any header including <cmath>)
|
||||
#ifndef _USE_MATH_DEFINES
|
||||
#define _USE_MATH_DEFINES
|
||||
#endif
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
#include <functional>
|
||||
|
||||
#include <rtabmap/core/ImuMotionPredictor.h>
|
||||
#include <rtabmap/core/IMU.h>
|
||||
|
||||
using rtabmap::ImuMotionPredictor;
|
||||
|
||||
namespace {
|
||||
|
||||
Eigen::Quaterniond yaw(double angle)
|
||||
{
|
||||
return Eigen::Quaterniond(Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()));
|
||||
}
|
||||
|
||||
rtabmap::Transform pose(const Eigen::Vector3d & position, double angle)
|
||||
{
|
||||
return rtabmap::Transform(position.x(), position.y(), position.z(), 0, 0, angle);
|
||||
}
|
||||
|
||||
/// An IMU at rest or accelerating, as measured: orientation of the IMU in its world
|
||||
/// frame, and the specific force (acceleration minus gravity) in the IMU frame.
|
||||
rtabmap::IMU measuredImu(const Eigen::Quaterniond & worldToImu,
|
||||
const Eigen::Vector3d & accelerationInWorld, double gravity,
|
||||
const rtabmap::Transform & baseToImu)
|
||||
{
|
||||
const Eigen::Vector3d f = worldToImu.inverse() * (accelerationInWorld + Eigen::Vector3d(0, 0, gravity));
|
||||
const Eigen::Quaterniond q = worldToImu.normalized();
|
||||
return rtabmap::IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat::eye(3,3,CV_64FC1),
|
||||
cv::Vec3d(0,0,0), cv::Mat::eye(3,3,CV_64FC1),
|
||||
cv::Vec3d(f.x(), f.y(), f.z()), cv::Mat::eye(3,3,CV_64FC1),
|
||||
baseToImu);
|
||||
}
|
||||
|
||||
/// IMU measurements at 200 Hz over [from, to] of an IMU at the base origin, oriented and
|
||||
/// accelerating (in its world frame) as given. Without accelerometer, it measures no
|
||||
/// linear acceleration at all. The stamps are on the 200 Hz grid (from must be too).
|
||||
void addSamples(ImuMotionPredictor & predictor, double from, double to,
|
||||
const std::function<Eigen::Quaterniond(double)> & orientation,
|
||||
const std::function<Eigen::Vector3d(double)> & acceleration,
|
||||
bool withAccelerometer = true)
|
||||
{
|
||||
// An integer over 200 rather than from + i*0.005: with FMA (e.g., -march=x86-64-v3),
|
||||
// -0.05 + 10*0.005 gives -1.7e-18, not 0, putting that sample on the wrong side of
|
||||
// a step at t=0.
|
||||
const double first = std::round(from * 200.0);
|
||||
for(int i=0; (first + i) / 200.0 <= to + 1e-9; ++i)
|
||||
{
|
||||
const double t = (first + i) / 200.0;
|
||||
rtabmap::IMU imu = measuredImu(orientation(t), acceleration(t), predictor.gravity(), rtabmap::Transform::getIdentity());
|
||||
if(!withAccelerometer)
|
||||
{
|
||||
imu = rtabmap::IMU(imu.orientation(), imu.orientationCovariance(),
|
||||
imu.angularVelocity(), imu.angularVelocityCovariance(),
|
||||
cv::Vec3d(0,0,0), imu.linearAccelerationCovariance(),
|
||||
imu.localTransform());
|
||||
}
|
||||
predictor.addImu(t, imu);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST(ImuMotionPredictor, predicts_only_the_orientation_without_a_pose)
|
||||
{
|
||||
ImuMotionPredictor predictor;
|
||||
EXPECT_TRUE(predictor.predict(1.0).isNull());
|
||||
|
||||
predictor.addImu(1.0, measuredImu(yaw(0.3), Eigen::Vector3d(1, 0, 0), 9.80665, rtabmap::Transform::getIdentity()));
|
||||
predictor.addImu(1.1, measuredImu(yaw(0.5), Eigen::Vector3d(1, 0, 0), 9.80665, rtabmap::Transform::getIdentity()));
|
||||
const rtabmap::Transform predicted = predictor.predict(1.05);
|
||||
ASSERT_FALSE(predicted.isNull()) << "the orientation is known without a pose";
|
||||
EXPECT_NEAR(predicted.theta(), 0.4, 1e-5);
|
||||
EXPECT_NEAR(predicted.x(), 0.0, 1e-9) << "no position without a pose";
|
||||
|
||||
ImuMotionPredictor withoutImu;
|
||||
withoutImu.addPose(1.0, rtabmap::Transform::getIdentity());
|
||||
EXPECT_TRUE(withoutImu.predict(1.0).isNull()) << "no imu yet";
|
||||
}
|
||||
|
||||
TEST(ImuMotionPredictor, follows_a_constant_velocity)
|
||||
{
|
||||
ImuMotionPredictor predictor;
|
||||
const Eigen::Vector3d velocity(1.0, -0.5, 0.2);
|
||||
addSamples(predictor, 0.0, 0.3,
|
||||
[](double) { return Eigen::Quaterniond::Identity(); },
|
||||
[](double) { return Eigen::Vector3d::Zero(); });
|
||||
predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), 0));
|
||||
EXPECT_TRUE(predictor.velocity().isZero()) << "a single pose has no velocity";
|
||||
predictor.addPose(0.1, pose(velocity * 0.1, 0));
|
||||
|
||||
EXPECT_TRUE(predictor.velocity().isApprox(velocity, 1e-6));
|
||||
const rtabmap::Transform predicted = predictor.predict(0.25);
|
||||
ASSERT_FALSE(predicted.isNull());
|
||||
EXPECT_NEAR(predicted.x(), velocity.x() * 0.25, 1e-6);
|
||||
EXPECT_NEAR(predicted.y(), velocity.y() * 0.25, 1e-6);
|
||||
EXPECT_NEAR(predicted.z(), velocity.z() * 0.25, 1e-6);
|
||||
}
|
||||
|
||||
TEST(ImuMotionPredictor, integrates_the_acceleration)
|
||||
{
|
||||
// From rest at t=0 with a constant 2 m/s^2: p = t^2, v = 2t. The velocity at the
|
||||
// second pose is the instantaneous one, not the average over the interval, and the
|
||||
// prediction keeps accelerating. An IMU without accelerometer gives a constant
|
||||
// velocity instead.
|
||||
const double a = 2.0;
|
||||
for(bool withAccelerometer : {true, false})
|
||||
{
|
||||
ImuMotionPredictor predictor;
|
||||
addSamples(predictor, -0.05, 0.3,
|
||||
[](double) { return Eigen::Quaterniond::Identity(); },
|
||||
[&](double t) { return Eigen::Vector3d(t < 0.0 ? 0.0 : a, 0, 0); },
|
||||
withAccelerometer);
|
||||
predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), 0));
|
||||
predictor.addPose(0.1, pose(Eigen::Vector3d(0.5*a*0.01, 0, 0), 0));
|
||||
|
||||
const double t = 0.2;
|
||||
const rtabmap::Transform predicted = predictor.predict(t);
|
||||
ASSERT_FALSE(predicted.isNull());
|
||||
if(withAccelerometer)
|
||||
{
|
||||
EXPECT_NEAR(predictor.velocity().x(), a*0.1, 1e-6);
|
||||
EXPECT_NEAR(predicted.x(), 0.5*a*t*t, 1e-6);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Constant velocity model: the average velocity over the last interval.
|
||||
EXPECT_NEAR(predictor.velocity().x(), 0.5*a*0.1, 1e-6);
|
||||
EXPECT_NEAR(predicted.x(), 0.5*a*0.01 + 0.5*a*0.1*(t-0.1), 1e-6);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST(ImuMotionPredictor, expresses_the_imu_in_the_odometry_frame)
|
||||
{
|
||||
// The IMU's world frame and the odometry frame differ by 90 degrees of yaw. The base
|
||||
// turns at 1 rad/s and accelerates along the IMU world's x, which is the odometry's y.
|
||||
const double rate = 1.0;
|
||||
const double offset = M_PI/2.0;
|
||||
const double a = 3.0;
|
||||
ImuMotionPredictor predictor;
|
||||
addSamples(predictor, 0.0, 0.3,
|
||||
[&](double t) { return yaw(rate*t); },
|
||||
[&](double) { return Eigen::Vector3d(a, 0, 0); });
|
||||
predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), offset));
|
||||
predictor.addPose(0.1, pose(Eigen::Vector3d(0, 0.5*a*0.01, 0), offset + rate*0.1));
|
||||
|
||||
const double t = 0.25;
|
||||
const rtabmap::Transform predicted = predictor.predict(t);
|
||||
ASSERT_FALSE(predicted.isNull());
|
||||
EXPECT_NEAR(predicted.x(), 0.0, 1e-6);
|
||||
EXPECT_NEAR(predicted.y(), 0.5*a*t*t, 1e-6);
|
||||
EXPECT_NEAR(predicted.theta(), offset + rate*t, 1e-5);
|
||||
}
|
||||
|
||||
TEST(ImuMotionPredictor, a_lost_pose_resets_the_prediction)
|
||||
{
|
||||
ImuMotionPredictor predictor;
|
||||
addSamples(predictor, 0.0, 0.5,
|
||||
[](double) { return Eigen::Quaterniond::Identity(); },
|
||||
[](double) { return Eigen::Vector3d::Zero(); });
|
||||
predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), 0));
|
||||
predictor.addPose(0.1, pose(Eigen::Vector3d(0.1, 0, 0), 0));
|
||||
ASSERT_FALSE(predictor.velocity().isZero());
|
||||
|
||||
predictor.addPose(0.2, rtabmap::Transform());
|
||||
EXPECT_TRUE(predictor.predict(0.25).isIdentity()) << "orientation only, which is constant here";
|
||||
|
||||
// After a reset of the odometry, the pose jumps: no velocity across it.
|
||||
predictor.addPose(0.3, pose(Eigen::Vector3d(10, 0, 0), 0));
|
||||
EXPECT_TRUE(predictor.velocity().isZero());
|
||||
EXPECT_NEAR(predictor.predict(0.4).x(), 10.0, 1e-6);
|
||||
}
|
||||
|
||||
TEST(ImuMotionPredictor, poses_too_far_apart_give_no_velocity)
|
||||
{
|
||||
ImuMotionPredictor predictor(0.5);
|
||||
addSamples(predictor, 0.0, 1.5,
|
||||
[](double) { return Eigen::Quaterniond::Identity(); },
|
||||
[](double) { return Eigen::Vector3d::Zero(); });
|
||||
predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), 0));
|
||||
predictor.addPose(1.0, pose(Eigen::Vector3d(1, 0, 0), 0));
|
||||
EXPECT_TRUE(predictor.velocity().isZero());
|
||||
}
|
||||
|
||||
TEST(ImuMotionPredictor, a_longer_window_averages_out_the_pose_noise)
|
||||
{
|
||||
// 1 m/s along x, with odometry poses alternating 1 cm on each side of the truth: the
|
||||
// worst case for a velocity differenced over one frame, which sees 0.2 m/s of noise.
|
||||
for(double window : {0.0, 0.5})
|
||||
{
|
||||
ImuMotionPredictor predictor(1.0, window);
|
||||
addSamples(predictor, 0.0, 2.0,
|
||||
[](double) { return Eigen::Quaterniond::Identity(); },
|
||||
[](double) { return Eigen::Vector3d::Zero(); });
|
||||
double maxError = 0.0;
|
||||
for(int i=0; i<=15; ++i)
|
||||
{
|
||||
const double t = i*0.1;
|
||||
predictor.addPose(t, pose(Eigen::Vector3d(t + (i%2?0.01:-0.01), 0, 0), 0));
|
||||
if(i >= 10)
|
||||
{
|
||||
maxError = std::max(maxError, std::fabs(predictor.velocity().x() - 1.0));
|
||||
}
|
||||
}
|
||||
if(window == 0.0)
|
||||
{
|
||||
EXPECT_NEAR(maxError, 0.2, 1e-6) << "differenced over one frame";
|
||||
}
|
||||
else
|
||||
{
|
||||
EXPECT_LT(maxError, 0.05) << "differenced over half a second";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST(ImuMotionPredictor, keeps_only_the_samples_since_the_last_pose)
|
||||
{
|
||||
ImuMotionPredictor predictor;
|
||||
addSamples(predictor, 0.0, 0.2,
|
||||
[](double) { return Eigen::Quaterniond::Identity(); },
|
||||
[](double) { return Eigen::Vector3d::Zero(); });
|
||||
ASSERT_EQ(predictor.samples(), 41u);
|
||||
predictor.addPose(0.1025, pose(Eigen::Vector3d::Zero(), 0));
|
||||
// 0.100 (the last one before the pose, to interpolate at its stamp) .. 0.200
|
||||
EXPECT_EQ(predictor.samples(), 21u);
|
||||
|
||||
predictor.reset();
|
||||
EXPECT_EQ(predictor.samples(), 0u);
|
||||
EXPECT_TRUE(predictor.predict(0.2).isNull());
|
||||
}
|
||||
|
||||
|
||||
TEST(ImuMotionPredictor, removes_gravity_from_what_the_imu_measures)
|
||||
{
|
||||
// The IMU is mounted rolled by 90 degrees on a level base at rest: it measures gravity
|
||||
// along its own y. Once removed, nothing moves, and the base stays level.
|
||||
const rtabmap::Transform baseToImu(0, 0, 0, M_PI/2.0, 0, 0);
|
||||
const Eigen::Quaterniond worldToImu = baseToImu.getQuaterniond();
|
||||
ImuMotionPredictor predictor;
|
||||
for(int i=0; i<=60; ++i)
|
||||
{
|
||||
predictor.addImu(i*0.005, measuredImu(worldToImu, Eigen::Vector3d::Zero(), 9.80665, baseToImu));
|
||||
}
|
||||
predictor.addPose(0.0, rtabmap::Transform::getIdentity());
|
||||
const rtabmap::Transform predicted = predictor.predict(0.3);
|
||||
ASSERT_FALSE(predicted.isNull());
|
||||
EXPECT_NEAR(predicted.getNorm(), 0.0, 1e-6) << "gravity was not removed";
|
||||
EXPECT_TRUE(predicted.getQuaterniond().isApprox(Eigen::Quaterniond::Identity(), 1e-6)) << "the base orientation is the imu's, unmounted";
|
||||
}
|
||||
|
||||
TEST(ImuMotionPredictor, removes_the_gravity_it_is_given)
|
||||
{
|
||||
// On the Moon, at rest: the IMU measures 1.62 m/s^2 up.
|
||||
ImuMotionPredictor predictor(1.0, 0.5, 1.62);
|
||||
for(int i=0; i<=60; ++i)
|
||||
{
|
||||
predictor.addImu(i*0.005, measuredImu(Eigen::Quaterniond::Identity(), Eigen::Vector3d::Zero(), 1.62,
|
||||
rtabmap::Transform::getIdentity()));
|
||||
}
|
||||
predictor.addPose(0.0, rtabmap::Transform::getIdentity());
|
||||
EXPECT_NEAR(predictor.predict(0.3).z(), 0.0, 1e-6);
|
||||
|
||||
// Earth's gravity removed from the same measurement: it looks like falling.
|
||||
ImuMotionPredictor earth;
|
||||
for(int i=0; i<=60; ++i)
|
||||
{
|
||||
earth.addImu(i*0.005, measuredImu(Eigen::Quaterniond::Identity(), Eigen::Vector3d::Zero(), 1.62,
|
||||
rtabmap::Transform::getIdentity()));
|
||||
}
|
||||
earth.addPose(0.0, rtabmap::Transform::getIdentity());
|
||||
EXPECT_NEAR(earth.predict(0.2).z(), 0.5*(1.62-9.80665)*0.04, 1e-6);
|
||||
}
|
||||
|
||||
TEST(ImuMotionPredictor, an_imu_without_acceleration_is_not_a_free_fall)
|
||||
{
|
||||
ImuMotionPredictor predictor;
|
||||
const Eigen::Quaterniond q(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ()));
|
||||
for(int i=0; i<=60; ++i)
|
||||
{
|
||||
predictor.addImu(i*0.005, rtabmap::IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat::eye(3,3,CV_64FC1),
|
||||
cv::Vec3d(0,0,0), cv::Mat::eye(3,3,CV_64FC1),
|
||||
cv::Vec3d(0,0,0), cv::Mat::eye(3,3,CV_64FC1),
|
||||
rtabmap::Transform::getIdentity()));
|
||||
}
|
||||
predictor.addPose(0.0, rtabmap::Transform::getIdentity());
|
||||
EXPECT_NEAR(predictor.predict(0.3).getNorm(), 0.0, 1e-6);
|
||||
|
||||
// And one without orientation is ignored altogether.
|
||||
ImuMotionPredictor noOrientation;
|
||||
noOrientation.addImu(0.0, rtabmap::IMU(cv::Vec4d(0,0,0,0), cv::Mat(),
|
||||
cv::Vec3d(0,0,0), cv::Mat(), cv::Vec3d(0,0,9.8), cv::Mat(), rtabmap::Transform::getIdentity()));
|
||||
EXPECT_EQ(noOrientation.samples(), 0u);
|
||||
}
|
||||
|
||||
TEST(ImuMotionPredictor, predicts_the_same_whatever_the_order_samples_and_predictions_come_in)
|
||||
{
|
||||
// The integration since the last pose is kept between predictions: it must not go
|
||||
// stale when samples arrive after a prediction, or out of order. Compared with a
|
||||
// predictor given every sample before predicting anything.
|
||||
auto acceleration = [](double t) { return Eigen::Vector3d(std::sin(20*t), std::cos(15*t), 0.3*t); };
|
||||
auto orientation = [](double t) { return yaw(0.5*t); };
|
||||
auto imu = [&](double t) { return measuredImu(orientation(t), acceleration(t), 9.80665, rtabmap::Transform::getIdentity()); };
|
||||
|
||||
ImuMotionPredictor reference;
|
||||
for(int i=0; i<=80; ++i) reference.addImu(i*0.005, imu(i*0.005));
|
||||
reference.addPose(0.0, rtabmap::Transform::getIdentity());
|
||||
reference.addPose(0.1, pose(Eigen::Vector3d(0.05, 0, 0), 0.05));
|
||||
|
||||
ImuMotionPredictor incremental;
|
||||
for(int i=0; i<=40; ++i) if(i != 30) incremental.addImu(i*0.005, imu(i*0.005));
|
||||
incremental.addPose(0.0, rtabmap::Transform::getIdentity());
|
||||
incremental.addPose(0.1, pose(Eigen::Vector3d(0.05, 0, 0), 0.05)); // velocity needs up to 0.1: covered
|
||||
incremental.predict(0.12); // integrates without the sample at 0.15
|
||||
incremental.predict(0.3); // beyond the newest sample (0.2)
|
||||
incremental.addImu(0.15, imu(0.15)); // out of order
|
||||
for(int i=41; i<=80; ++i)
|
||||
{
|
||||
incremental.addImu(i*0.005, imu(i*0.005));
|
||||
if(i % 7 == 0) incremental.predict(i*0.005 - 0.0012);
|
||||
}
|
||||
|
||||
for(double t : {0.1, 0.1013, 0.15, 0.2337, 0.4, 0.45})
|
||||
{
|
||||
const rtabmap::Transform a = reference.predict(t);
|
||||
const rtabmap::Transform b = incremental.predict(t);
|
||||
ASSERT_FALSE(a.isNull());
|
||||
ASSERT_FALSE(b.isNull());
|
||||
EXPECT_NEAR(a.x(), b.x(), 1e-6) << "t=" << t;
|
||||
EXPECT_NEAR(a.y(), b.y(), 1e-6) << "t=" << t;
|
||||
EXPECT_NEAR(a.z(), b.z(), 1e-6) << "t=" << t;
|
||||
}
|
||||
}
|
||||
|
||||
TEST(ImuMotionPredictor, holds_the_acceleration_at_the_pose_until_a_newer_sample)
|
||||
{
|
||||
// The pose comes after the newest sample: the acceleration there is held from that
|
||||
// sample, until a newer one says otherwise.
|
||||
ImuMotionPredictor reference;
|
||||
ImuMotionPredictor incremental;
|
||||
auto imu = [](double a) { return measuredImu(Eigen::Quaterniond::Identity(), Eigen::Vector3d(a, 0, 0), 9.80665, rtabmap::Transform::getIdentity()); };
|
||||
for(ImuMotionPredictor * p : {&reference, &incremental})
|
||||
{
|
||||
p->addImu(0.0, imu(0.0));
|
||||
p->addImu(0.1, imu(0.0));
|
||||
p->addPose(0.15, rtabmap::Transform::getIdentity());
|
||||
}
|
||||
incremental.predict(0.2);
|
||||
for(ImuMotionPredictor * p : {&reference, &incremental})
|
||||
{
|
||||
p->addImu(0.2, imu(4.0));
|
||||
p->addImu(0.3, imu(4.0));
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
namespace {
|
||||
|
||||
/// An IMU tilted by 0.1 rad in pitch whose accelerometer reads `measuredGravity` at rest,
|
||||
/// with a vibration of the given amplitude (m/s^2) on every axis and the given angular
|
||||
/// velocity (rad/s, about z).
|
||||
rtabmap::IMU tiltedImu(double t, double measuredGravity, double vibration = 0.05, double angularVelocity = 0.0)
|
||||
{
|
||||
const Eigen::Quaterniond worldToImu(Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitY()));
|
||||
const Eigen::Vector3d f = worldToImu.inverse() * Eigen::Vector3d(0, 0, measuredGravity) +
|
||||
vibration * Eigen::Vector3d(std::sin(t*331.0), std::sin(t*457.0), std::sin(t*563.0));
|
||||
return rtabmap::IMU(cv::Vec4d(worldToImu.x(), worldToImu.y(), worldToImu.z(), worldToImu.w()), cv::Mat::eye(3,3,CV_64FC1),
|
||||
cv::Vec3d(0, 0, angularVelocity), cv::Mat::eye(3,3,CV_64FC1),
|
||||
cv::Vec3d(f.x(), f.y(), f.z()), cv::Mat::eye(3,3,CV_64FC1),
|
||||
rtabmap::Transform::getIdentity());
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST(ImuMotionPredictor, estimates_the_gravity_while_the_imu_is_still)
|
||||
{
|
||||
// An accelerometer reading 9.55 m/s^2 at rest, as the netherdrone's does.
|
||||
ImuMotionPredictor predictor(1.0, 0.5, 0.0);
|
||||
EXPECT_TRUE(predictor.isGravityEstimated());
|
||||
EXPECT_DOUBLE_EQ(predictor.gravity(), 9.80665) << "standard gravity until estimated";
|
||||
|
||||
for(int i=0; i<100; ++i) // 0.5 s at 200 Hz: the first window is not complete yet
|
||||
{
|
||||
predictor.addImu(i*0.005, tiltedImu(i*0.005, 9.55));
|
||||
}
|
||||
EXPECT_EQ(predictor.gravityWindows(), 0u);
|
||||
EXPECT_DOUBLE_EQ(predictor.gravity(), 9.80665);
|
||||
|
||||
for(int i=100; i<=400; ++i)
|
||||
{
|
||||
predictor.addImu(i*0.005, tiltedImu(i*0.005, 9.55));
|
||||
}
|
||||
EXPECT_GE(predictor.gravityWindows(), 3u);
|
||||
EXPECT_NEAR(predictor.gravity(), 9.55, 0.005) << "whatever the attitude";
|
||||
|
||||
// At rest, it doesn't look like falling anymore.
|
||||
predictor.addPose(1.5, rtabmap::Transform::getIdentity());
|
||||
predictor.addPose(2.0, rtabmap::Transform::getIdentity());
|
||||
EXPECT_NEAR(predictor.predict(2.0).z(), 0.0, 1e-3);
|
||||
for(int i=401; i<=460; ++i)
|
||||
{
|
||||
predictor.addImu(i*0.005, tiltedImu(i*0.005, 9.55));
|
||||
}
|
||||
EXPECT_NEAR(predictor.predict(2.3).z(), 0.0, 2e-3);
|
||||
|
||||
// The estimate is the sensor's: a reset keeps it.
|
||||
predictor.reset();
|
||||
EXPECT_NEAR(predictor.gravity(), 9.55, 0.005);
|
||||
}
|
||||
|
||||
TEST(ImuMotionPredictor, does_not_take_a_moving_imu_for_a_still_one)
|
||||
{
|
||||
// Turning (centripetal acceleration, gravity moving between the axes) or vibrating
|
||||
// as in flight: not still, standard gravity is kept.
|
||||
ImuMotionPredictor turning(1.0, 0.5, 0.0);
|
||||
ImuMotionPredictor vibrating(1.0, 0.5, 0.0);
|
||||
for(int i=0; i<=400; ++i)
|
||||
{
|
||||
turning.addImu(i*0.005, tiltedImu(i*0.005, 9.55, 0.05, 0.5));
|
||||
vibrating.addImu(i*0.005, tiltedImu(i*0.005, 9.55, 1.0));
|
||||
}
|
||||
EXPECT_EQ(turning.gravityWindows(), 0u);
|
||||
EXPECT_DOUBLE_EQ(turning.gravity(), 9.80665);
|
||||
EXPECT_EQ(vibrating.gravityWindows(), 0u);
|
||||
EXPECT_DOUBLE_EQ(vibrating.gravity(), 9.80665);
|
||||
}
|
||||
|
||||
TEST(ImuMotionPredictor, keeps_the_gravity_it_is_given)
|
||||
{
|
||||
ImuMotionPredictor predictor;
|
||||
for(int i=0; i<=400; ++i)
|
||||
{
|
||||
predictor.addImu(i*0.005, tiltedImu(i*0.005, 9.55));
|
||||
}
|
||||
EXPECT_FALSE(predictor.isGravityEstimated());
|
||||
EXPECT_EQ(predictor.gravityWindows(), 0u);
|
||||
EXPECT_DOUBLE_EQ(predictor.gravity(), 9.80665);
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,20 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
|
||||
using rtabmap::Parameters;
|
||||
using rtabmap::ParametersMap;
|
||||
|
||||
TEST(Parameters, descriptions_are_shorter_than_1024_characters)
|
||||
{
|
||||
// Long descriptions don't fit in the GUI's tooltips nor in a ROS parameter listing:
|
||||
// keep them short, the details belong in the documentation. 1024 was also the size
|
||||
// beyond which uFormat(), which formats most of them, aborted on Windows.
|
||||
const ParametersMap & parameters = Parameters::getDefaultParameters();
|
||||
ASSERT_FALSE(parameters.empty());
|
||||
for(ParametersMap::const_iterator iter = parameters.begin(); iter != parameters.end(); ++iter)
|
||||
{
|
||||
const std::string description = Parameters::getDescription(iter->first);
|
||||
EXPECT_LT(description.size(), 1024u) << iter->first << " has a description of "
|
||||
<< description.size() << " characters: shorten it";
|
||||
}
|
||||
}
|
||||
@@ -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