9#include <gtsam/geometry/Pose3.h>
17void require(
bool condition,
const std::string& message) {
19 throw std::runtime_error(message);
24const gtsam::Pose3 kT_I_S(gtsam::Rot3::Ypr(0.5, 0.6, M_PI), gtsam::Point3(0.2, -0.1, 0.3));
30 setInitializationFrame(
"imu");
31 setBaseLinkFrame(
"imu");
32 setWorldFrame(
"world");
34 set_T_frame1_frame2_andInverse(
"imu",
"sensor", Eigen::Isometry3d(kT_I_S.matrix()));
37 bool findTransformations()
override {
return true; }
43gtsam::Pose3 initializeImuPose(
const gtsam::Rot3& R_W_I,
const std::function<
bool(TestEstimator&)>& init) {
44 auto config = std::make_shared<holistic_fusion::GraphConfig>();
45 config->imuRate_ = 10.0;
46 config->imuBufferLength_ = 20;
47 config->useImuSignalLowPassFilter_ =
false;
48 config->staticAtStartup_ =
true;
49 config->gyroBiasPrior_ = Eigen::Vector3d::Zero();
50 config->estimateGravityFromImuFlag_ =
true;
51 config->optimizeReferenceFramePosesWrtWorldFlag_ =
true;
52 TestEstimator estimator;
53 estimator.setup(config, std::make_shared<TestTransforms>());
55 std::shared_ptr<holistic_fusion::SafeIntegratedNavState> state;
56 std::shared_ptr<holistic_fusion::SafeNavStateWithCovarianceAndBias> optimizedState;
57 Eigen::Matrix<double, 6, 1> measurements;
58 const Eigen::Vector3d I_specificForce = R_W_I.unrotate(gtsam::Point3(0.0, 0.0, 9.81));
59 double timestamp = 1.0;
60 const auto addImu = [&]() {
62 estimator.addCoreImuMeasurementAndGetState(I_specificForce, Eigen::Vector3d::Zero(), timestamp, state, optimizedState, measurements);
64 for (
int index = 0; index < 20 && !estimator.areRollAndPitchInited(); ++index) {
67 require(estimator.areRollAndPitchInited(),
"Stationary IMU alignment failed");
68 require(init(estimator),
"Initialization failed");
69 require(!init(estimator),
"A second initialization must fail");
71 return gtsam::Pose3(state->getT_W_Ik().matrix());
75 const std::string& fixedFrame) {
83 Eigen::Isometry3d(T_M_S.matrix()),
84 Eigen::Matrix<double, 6, 1>::Ones(),
87 Eigen::Matrix<double, 6, 1>::Ones()};
90double headingError(
const gtsam::Rot3& R_W_S,
const gtsam::Rot3& R_W_Smeas) {
91 return gtsam::Rot3::Logmap(R_W_S * R_W_Smeas.inverse()).z();
95void headingInitMatchesMeasuredPoseOfSensor() {
96 const gtsam::Rot3 xAxisUp = gtsam::Rot3::Ry(-M_PI_2);
97 const std::vector<gtsam::Rot3> sensorAttitudes{gtsam::Rot3::Ypr(2.1, 0.2, -0.4), gtsam::Rot3::Rz(-2.8) * xAxisUp,
98 gtsam::Rot3::Rz(0.4) * gtsam::Rot3::Rx(M_PI)};
99 for (
const gtsam::Rot3& R_W_S : sensorAttitudes) {
100 for (
const gtsam::Rot3& tiltError : {gtsam::Rot3(), gtsam::Rot3::Rx(0.02) * gtsam::Rot3::Ry(-0.015)}) {
101 const gtsam::Pose3 T_W_Smeas(tiltError * R_W_S, gtsam::Point3(1.0, -2.0, 0.5));
102 const gtsam::Pose3 T_W_I = initializeImuPose(R_W_S * kT_I_S.rotation().inverse(), [&](TestEstimator& estimator) {
103 return estimator.initHeadingAndPosition(poseMeasurement(T_W_Smeas,
"world"));
105 const gtsam::Pose3 T_W_S = T_W_I * kT_I_S;
107 require(std::abs(headingError(T_W_S.rotation(), T_W_Smeas.rotation())) < 1e-6,
"Heading of S does not match the measurement");
108 require(T_W_S.translation().isApprox(T_W_Smeas.translation(), 1e-6),
"Position of S does not match the measurement");
109 if (tiltError.equals(gtsam::Rot3())) {
110 require(T_W_S.rotation().equals(R_W_S, 1e-6),
"Orientation of S does not match a consistent measurement");
117void headingInitGoesThroughFixedFrameGuess() {
118 const gtsam::Pose3 T_W_M(gtsam::Rot3::Rz(0.7), gtsam::Point3(3.0, 1.0, -0.5));
119 const gtsam::Rot3 R_W_S = gtsam::Rot3::Ypr(-1.3, 0.1, 0.3);
120 const gtsam::Pose3 T_M_Smeas = T_W_M.inverse() * gtsam::Pose3(R_W_S, gtsam::Point3(-1.0, 4.0, 0.2));
121 const gtsam::Pose3 T_W_I = initializeImuPose(R_W_S * kT_I_S.rotation().inverse(), [&](TestEstimator& estimator) {
122 estimator.initWorldFrameToFixedFrameTransform(Eigen::Isometry3d(T_W_M.matrix()),
"map");
123 return estimator.initHeadingAndPosition(poseMeasurement(T_M_Smeas,
"map"));
126 require((T_W_I * kT_I_S).equals(T_W_M * T_M_Smeas, 1e-6),
"Pose of S does not match the measurement through T_W_M");
138 Eigen::Matrix<double, 1, 1>::Ones(),
141 Eigen::Matrix<double, 6, 1>::Ones()};
145void yawInitMatchesMeasuredYawOfSensor() {
146 const gtsam::Pose3 T_W_M(gtsam::Rot3::Ypr(0.3, 0.05, -0.04), gtsam::Point3(3.0, 1.0, -0.5));
147 const gtsam::Rot3 R_W_I = gtsam::Rot3::Ypr(-0.9, 0.25, -0.3);
148 const double yaw_W_S1meas = 1.2;
149 const Eigen::Vector3d M_t_M_S2(0.5, -1.5, 0.3);
151 "position", 10,
"imu",
"imu_corrected",
holistic_fusion::RobustNorm::None(), 1.0, 1.0, M_t_M_S2, Eigen::Vector3d::Ones(),
"map",
152 "world", Eigen::Matrix<double, 6, 1>::Ones());
153 const gtsam::Pose3 T_W_Iinit = initializeImuPose(R_W_I, [&](TestEstimator& estimator) {
154 estimator.initWorldFrameToFixedFrameTransform(Eigen::Isometry3d(T_W_M.matrix()),
"map");
155 return estimator.initYawAndPosition(yawMeasurement(yaw_W_S1meas,
"world"), positionMeasurement);
158 const double yaw_W_S1 = (T_W_Iinit.rotation() * kT_I_S.rotation()).yaw();
159 require(std::abs(yaw_W_S1 - yaw_W_S1meas) < 1e-6,
"Yaw of S1 does not match: " + std::to_string(yaw_W_S1));
160 require(std::abs(T_W_Iinit.rotation().pitch() - R_W_I.pitch()) < 1e-6 && std::abs(T_W_Iinit.rotation().roll() - R_W_I.roll()) < 1e-6,
161 "Roll and pitch do not stay with gravity");
162 require(T_W_Iinit.translation().isApprox(T_W_M.transformFrom(gtsam::Point3(M_t_M_S2)), 1e-6),
"Position of S2 does not match");
165void yawInitRejectsYawInFixedFrame() {
168 Eigen::Vector3d::Ones(),
"world",
"world");
169 bool rejected =
false;
171 initializeImuPose(gtsam::Rot3(), [&](TestEstimator& estimator) {
172 return estimator.initYawAndPosition(yawMeasurement(0.0,
"map"), positionMeasurement);
174 }
catch (
const std::invalid_argument&) {
177 require(rejected,
"A yaw in a fixed frame must be rejected");
180void identityInitKeepsOnlyTheGravityTilt() {
181 const gtsam::Rot3 R_W_I = gtsam::Rot3::Ypr(1.1, -0.2, 0.15);
182 const gtsam::Pose3 T_W_I =
183 initializeImuPose(R_W_I, [](TestEstimator& estimator) {
return estimator.initHeadingAndPositionAsIdentity(); });
185 require(T_W_I.equals(gtsam::Pose3(gtsam::Rot3::Ypr(0.0, -0.2, 0.15), gtsam::Point3::Zero()), 1e-6),
186 "Heading and position are not identity");
193 headingInitMatchesMeasuredPoseOfSensor();
194 headingInitGoesThroughFixedFrameGuess();
195 yawInitMatchesMeasuredYawOfSensor();
196 yawInitRejectsYawInFixedFrame();
197 identityInitKeepsOnlyTheGravityTilt();
198 }
catch (
const std::exception& error) {
199 std::cerr << error.what() <<
'\n';
Class to represent x-dimensional absolute unary measurement in the graph.