My Project 1.0
Holistic Fusion: A Generic Framework for Multimodal Sensor Fusion
Loading...
Searching...
No Matches
test_init_heading_and_position.cpp
Go to the documentation of this file.
1#include <cmath>
2#include <functional>
3#include <iostream>
4#include <memory>
5#include <stdexcept>
6#include <string>
7#include <vector>
8
9#include <gtsam/geometry/Pose3.h>
10
14
15namespace {
16
17void require(bool condition, const std::string& message) {
18 if (!condition) {
19 throw std::runtime_error(message);
20 }
21}
22
23// Sensor frame S with a strongly tilted mount on the IMU, where a yaw transfer that ignores the tilt fails
24const gtsam::Pose3 kT_I_S(gtsam::Rot3::Ypr(0.5, 0.6, M_PI), gtsam::Point3(0.2, -0.1, 0.3));
25
26class TestTransforms final : public holistic_fusion::StaticTransforms {
27 public:
28 TestTransforms() {
29 setImuFrame("imu");
30 setInitializationFrame("imu");
31 setBaseLinkFrame("imu");
32 setWorldFrame("world");
33 setOdomFrame("odom");
34 set_T_frame1_frame2_andInverse("imu", "sensor", Eigen::Isometry3d(kT_I_S.matrix()));
35 }
36
37 bool findTransformations() override { return true; }
38};
39
41
42// Aligns a static IMU with the true orientation R_W_I, then runs the init and returns the initialized IMU pose in the world
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>());
54
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 = [&]() {
61 timestamp += 0.1;
62 estimator.addCoreImuMeasurementAndGetState(I_specificForce, Eigen::Vector3d::Zero(), timestamp, state, optimizedState, measurements);
63 };
64 for (int index = 0; index < 20 && !estimator.areRollAndPitchInited(); ++index) {
65 addImu();
66 }
67 require(estimator.areRollAndPitchInited(), "Stationary IMU alignment failed");
68 require(init(estimator), "Initialization failed");
69 require(!init(estimator), "A second initialization must fail");
70 addImu();
71 return gtsam::Pose3(state->getT_W_Ik().matrix());
72}
73
74holistic_fusion::UnaryMeasurementXDAbsolute<Eigen::Isometry3d, 6> poseMeasurement(const gtsam::Pose3& T_M_S,
75 const std::string& fixedFrame) {
76 return {"pose",
77 10,
78 "sensor",
79 "sensor_corrected",
81 1.0,
82 1.0,
83 Eigen::Isometry3d(T_M_S.matrix()),
84 Eigen::Matrix<double, 6, 1>::Ones(),
85 fixedFrame,
86 "world",
87 Eigen::Matrix<double, 6, 1>::Ones()};
88}
89
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();
92}
93
94// The heading of S must match the measurement in every attitude, also if the measured tilt differs slightly from the IMU
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"));
104 });
105 const gtsam::Pose3 T_W_S = T_W_I * kT_I_S;
106
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");
111 }
112 }
113 }
114}
115
116// In a fixed frame M, the measurement goes through the guess of T_W_M
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"));
124 });
125
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");
127}
128
129holistic_fusion::UnaryMeasurementXDAbsolute<double, 1> yawMeasurement(const double yaw, const std::string& fixedFrame) {
130 return {"yaw",
131 10,
132 "sensor",
133 "sensor_corrected",
135 1.0,
136 1.0,
137 yaw,
138 Eigen::Matrix<double, 1, 1>::Ones(),
139 fixedFrame,
140 "world",
141 Eigen::Matrix<double, 6, 1>::Ones()};
142}
143
144// The yaw of S1 in the world and the position of S2 through T_W_M must match, while roll and pitch stay with gravity
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);
156 });
157
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");
163}
164
165void yawInitRejectsYawInFixedFrame() {
167 "position", 10, "imu", "imu_corrected", holistic_fusion::RobustNorm::None(), 1.0, 1.0, Eigen::Vector3d::Zero(),
168 Eigen::Vector3d::Ones(), "world", "world");
169 bool rejected = false;
170 try {
171 initializeImuPose(gtsam::Rot3(), [&](TestEstimator& estimator) {
172 return estimator.initYawAndPosition(yawMeasurement(0.0, "map"), positionMeasurement);
173 });
174 } catch (const std::invalid_argument&) {
175 rejected = true;
176 }
177 require(rejected, "A yaw in a fixed frame must be rejected");
178}
179
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(); });
184
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");
187}
188
189} // namespace
190
191int main() {
192 try {
193 headingInitMatchesMeasuredPoseOfSensor();
194 headingInitGoesThroughFixedFrameGuess();
195 yawInitMatchesMeasuredYawOfSensor();
196 yawInitRejectsYawInFixedFrame();
197 identityInitKeepsOnlyTheGravityTilt();
198 } catch (const std::exception& error) {
199 std::cerr << error.what() << '\n';
200 return 1;
201 }
202 return 0;
203}
Class to represent x-dimensional absolute unary measurement in the graph.
static RobustNorm None()
Definition Measurement.h:33