Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp
Go to the documentation of this file.
1 
10 #include <gtsam/slam/expressions.h>
13 #include <gtsam/nonlinear/Values.h>
14 #include <examples/SFMdata.h>
15 
16 using namespace gtsam;
17 
19 
20 /* ************************************************************************* */
21 int 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(2*M_PI/8).Pitch(M_PI/8), Point3(1, 0, 0));
26  Pose3 delta_pose2 = Pose3(Rot3().Pitch(-M_PI/8), Point3(1, 0, 0));
27  Pose3 delta_pose3 = Pose3(Rot3().Yaw(-2*M_PI/8), Point3(1, 0, 0));
28 
29  int steps = 4;
30  auto poses = createPoses(init_pose, delta_pose1, steps);
31  auto poses2 = createPoses(init_pose, delta_pose2, steps);
32  auto poses3 = createPoses(init_pose, delta_pose3, steps);
33 
34  // Concatenate poses to create trajectory
35  poses.insert( poses.end(), poses2.begin(), poses2.end() );
36  poses.insert( poses.end(), poses3.begin(), 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(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
41 
42  // The graph
44 
45  // Specify uncertainty on first pose prior and also for between factor (simplicity reasons)
46  auto poseNoise = noiseModel::Diagonal::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((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(x_, poses[0], 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(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(points[j]), world_T_sensor.range(points[j]));
68 
69  // Add factor
70  graph.addExpressionFactor(prediction_, measurement, 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(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise);
78  }
79  }
80 
81  // Create perturbed initial
83  Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
84  for (size_t i = 0; i < poses.size(); ++i)
85  initial.insert(Symbol('x', i), poses[i].compose(delta));
86  for (size_t j = 0; j < points.size(); ++j)
87  initial.insert<Point3>(Symbol('l', j), 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>(Symbol('T',0), Pose3());
91 
92  std::cout << "initial error: " << graph.error(initial) << std::endl;
94  std::cout << "final error: " << graph.error(result) << std::endl;
95 
96  initial.at<Pose3>(Symbol('T',0)).print("\nInitial estimate body_T_sensor\n"); /* initial sensor_P_body estimate */
97  result.at<Pose3>(Symbol('T',0)).print("\nFinal estimate body_T_sensor\n"); /* optimized sensor_P_body estimate */
98  body_T_sensor_gt.print("\nGround truth body_T_sensor\n"); /* sensor_P_body ground truth */
99 
100  return 0;
101 }
102 /* ************************************************************************* */
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
static Rot3 RzRyRx(double x, double y, double z, OptionalJacobian< 3, 1 > Hx={}, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hz={})
Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
Definition: Rot3M.cpp:84
virtual const Values & optimize()
A non-templated config holding any types of Manifold-group elements.
const ValueType at(Key j) const
Definition: Values-inl.h:204
#define M_PI
Definition: main.h:106
BearingRange< Pose3, Point3 > BearingRange3D
Values initial
int main(int argc, char *argv[])
NonlinearFactorGraph graph
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
std::vector< gtsam::Point3 > createPoints()
Definition: SFMdata.h:39
static Point2 measurement(323.0, 240.0)
static Rot3 Rodrigues(const Vector3 &w)
Definition: Rot3.h:240
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
Factor graph that supports adding ExpressionFactors directly.
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
double error(const Values &values) const
Expression< Point3 > Point3_
traits
Definition: chartTesting.h:28
void print(const std::string &s="") const
print with optional string
Definition: Pose3.cpp:152
Bearing-Range product.
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:270
Expression< T > compose(const Expression< T > &t1, const Expression< T > &t2)
Simple example for the structure-from-motion problems.
void insert(Key j, const Value &val)
Definition: Values.cpp:155
void addExpressionFactor(const Expression< T > &h, const T &z, const SharedNoiseModel &R)
Vector3 Point3
Definition: Point3.h:38
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
std::ptrdiff_t j
Expression< Pose3 > Pose3_
std::vector< gtsam::Pose3 > createPoses(const gtsam::Pose3 &init=gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2, 0,-M_PI/2), gtsam::Point3(30, 0, 0)), const gtsam::Pose3 &delta=gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4, 0), gtsam::Point3(sin(M_PI/4) *30, 0, 30 *(1-sin(M_PI/4)))), int steps=8)
Definition: SFMdata.h:56


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:15