1/**
2 * @file Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp
3 * @brief A simultaneous optimization of trajectory, landmarks and sensor-pose with respect to body-pose using bearing-range measurements done with Expressions
4 * @author Thomas Horstink
5 * @date January 4th, 2019
6 */
7
8#include <gtsam/inference/Symbol.h>
9#include <gtsam/geometry/BearingRange.h>
10#include <gtsam/slam/expressions.h>
11#include <gtsam/nonlinear/ExpressionFactorGraph.h>
12#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
13#include <gtsam/nonlinear/Values.h>
14#include <examples/SFMdata.h>
15
16using namespace gtsam;
17
18typedef BearingRange<Pose3, Point3> BearingRange3D;
19
20/* ************************************************************************* */
21int main(int argc, char* argv[]) {
22
23 // Move around so the whole state (including the sensor tf) is observable
24 Pose3 init_pose = Pose3();
25 Pose3 delta_pose1 = Pose3(Rot3().Yaw(t: 2*M_PI/8).Pitch(M_PI/8), Point3(1, 0, 0));
26 Pose3 delta_pose2 = Pose3(Rot3().Pitch(t: -M_PI/8), Point3(1, 0, 0));
27 Pose3 delta_pose3 = Pose3(Rot3().Yaw(t: -2*M_PI/8), Point3(1, 0, 0));
28
29 int steps = 4;
30 auto poses = createPoses(init: init_pose, delta: delta_pose1, steps);
31 auto poses2 = createPoses(init: init_pose, delta: delta_pose2, steps);
32 auto poses3 = createPoses(init: init_pose, delta: delta_pose3, steps);
33
34 // Concatenate poses to create trajectory
35 poses.insert( position: poses.end(), first: poses2.begin(), last: poses2.end() );
36 poses.insert( position: poses.end(), first: poses3.begin(), last: poses3.end() ); // std::vector of Pose3
37 auto points = createPoints(); // std::vector of Point3
38
39 // (ground-truth) sensor pose in body frame, further an unknown variable
40 Pose3 body_T_sensor_gt(Rot3::RzRyRx(x: -M_PI_2, y: 0.0, z: -M_PI_2), Point3(0.25, -0.10, 1.0));
41
42 // The graph
43 ExpressionFactorGraph graph;
44
45 // Specify uncertainty on first pose prior and also for between factor (simplicity reasons)
46 auto poseNoise = noiseModel::Diagonal::Sigmas(sigmas: (Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished());
47
48 // Uncertainty bearing range measurement;
49 auto bearingRangeNoise = noiseModel::Diagonal::Sigmas(sigmas: (Vector(3)<<0.01,0.03,0.05).finished());
50
51 // Expressions for body-frame at key 0 and sensor-tf
52 Pose3_ x_('x', 0);
53 Pose3_ body_T_sensor_('T', 0);
54
55 // Add a prior on the body-pose
56 graph.addExpressionFactor(h: x_, z: poses[0], R: poseNoise);
57
58 // Simulated measurements from pose
59 for (size_t i = 0; i < poses.size(); ++i) {
60 auto world_T_sensor = poses[i].compose(g: body_T_sensor_gt);
61 for (size_t j = 0; j < points.size(); ++j) {
62
63 // This expression is the key feature of this example: it creates a differentiable expression of the measurement after being displaced by sensor transform.
64 auto prediction_ = Expression<BearingRange3D>( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j));
65
66 // Create a *perfect* measurement
67 auto measurement = BearingRange3D(world_T_sensor.bearing(point: points[j]), world_T_sensor.range(point: points[j]));
68
69 // Add factor
70 graph.addExpressionFactor(h: prediction_, z: measurement, R: bearingRangeNoise);
71 }
72
73 // and add a between factor to the graph
74 if (i > 0)
75 {
76 // And also we have a *perfect* measurement for the between factor.
77 graph.addExpressionFactor(h: between(t1: Pose3_('x', i-1),t2: Pose3_('x', i)), z: poses[i-1].between(g: poses[i]), R: poseNoise);
78 }
79 }
80
81 // Create perturbed initial
82 Values initial;
83 Pose3 delta(Rot3::Rodrigues(wx: -0.1, wy: 0.2, wz: 0.25), Point3(0.05, -0.10, 0.20));
84 for (size_t i = 0; i < poses.size(); ++i)
85 initial.insert(j: Symbol('x', i), val: poses[i].compose(g: delta));
86 for (size_t j = 0; j < points.size(); ++j)
87 initial.insert<Point3>(j: Symbol('l', j), val: points[j] + Point3(-0.25, 0.20, 0.15));
88
89 // Initialize body_T_sensor wrongly (because we do not know!)
90 initial.insert<Pose3>(j: Symbol('T',0), val: Pose3());
91
92 std::cout << "initial error: " << graph.error(values: initial) << std::endl;
93 Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
94 std::cout << "final error: " << graph.error(values: result) << std::endl;
95
96 initial.at<Pose3>(j: Symbol('T',0)).print(s: "\nInitial estimate body_T_sensor\n"); /* initial sensor_P_body estimate */
97 result.at<Pose3>(j: Symbol('T',0)).print(s: "\nFinal estimate body_T_sensor\n"); /* optimized sensor_P_body estimate */
98 body_T_sensor_gt.print(s: "\nGround truth body_T_sensor\n"); /* sensor_P_body ground truth */
99
100 return 0;
101}
102/* ************************************************************************* */