Merged in thomashorstink/gtsam/feature/Pose3SLAMExampleExpressions_BearinRangeWithTransform (pull request #367)

Expression example of estimating trajectory, landmarks and sensor-body-transform simultaneously

examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp

@@ -0,0 +1,102 @@
+/**
+ * @file    Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp
+ * @brief   A simultaneous optimization of trajectory, landmarks and sensor-pose with respect to body-pose using bearing-range measurements done with Expressions
+ * @author  Thomas Horstink
+ * @date    January 4th, 2019
+ */
+
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/geometry/BearingRange.h>
+#include <gtsam/slam/expressions.h>
+#include <gtsam/nonlinear/ExpressionFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Values.h>
+#include <examples/SFMdata.h>
+
+using namespace gtsam;
+
+typedef BearingRange<Pose3, Point3> BearingRange3D;
+
+/* ************************************************************************* */
+int main(int argc, char* argv[]) {
+
+  // Move around so the whole state (including the sensor tf) is observable
+  Pose3 init_pose = Pose3();
+  Pose3 delta_pose1 = Pose3(Rot3().Yaw(2*M_PI/8).Pitch(M_PI/8), Point3(1, 0, 0));
+  Pose3 delta_pose2 = Pose3(Rot3().Pitch(-M_PI/8), Point3(1, 0, 0));
+  Pose3 delta_pose3 = Pose3(Rot3().Yaw(-2*M_PI/8), Point3(1, 0, 0));
+
+  int steps = 4;
+  auto poses  = createPoses(init_pose, delta_pose1, steps);
+  auto poses2 = createPoses(init_pose, delta_pose2, steps);
+  auto poses3 = createPoses(init_pose, delta_pose3, steps);
+
+  // Concatenate poses to create trajectory
+  poses.insert( poses.end(), poses2.begin(), poses2.end() );
+  poses.insert( poses.end(), poses3.begin(), poses3.end() );  // std::vector of Pose3
+  auto points = createPoints();                               // std::vector of Point3 
+
+  // (ground-truth) sensor pose in body frame, further an unknown variable
+  Pose3 body_T_sensor_gt(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
+  
+  // The graph
+  ExpressionFactorGraph graph;
+  
+  // Specify uncertainty on first pose prior and also for between factor (simplicity reasons)
+  auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished());
+  
+  // Uncertainty bearing range measurement;
+  auto bearingRangeNoise = noiseModel::Diagonal::Sigmas((Vector(3)<<0.01,0.03,0.05).finished());
+  
+  // Expressions for body-frame at key 0 and sensor-tf
+  Pose3_ x_('x', 0);
+  Pose3_ body_T_sensor_('T', 0);
+  
+  // Add a prior on the body-pose
+  graph.addExpressionFactor(x_, poses[0], poseNoise); 
+  
+  // Simulated measurements from pose
+  for (size_t i = 0; i < poses.size(); ++i) {
+    auto world_T_sensor = poses[i].compose(body_T_sensor_gt);
+    for (size_t j = 0; j < points.size(); ++j) {
+      
+      // This expression is the key feature of this example: it creates a differentiable expression of the measurement after being displaced by sensor transform.
+      auto prediction_ = Expression<BearingRange3D>( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j));
+      
+      // Create a *perfect* measurement
+      auto measurement = BearingRange3D(world_T_sensor.bearing(points[j]), world_T_sensor.range(points[j]));
+      
+      // Add factor
+      graph.addExpressionFactor(prediction_, measurement, bearingRangeNoise);
+    }
+    
+    // and add a between factor to the graph
+    if (i > 0)
+    {
+      // And also we have a *perfect* measurement for the between factor.
+      graph.addExpressionFactor(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise); 
+    }
+  }
+
+  // Create perturbed initial
+  Values initial;
+  Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
+  for (size_t i = 0; i < poses.size(); ++i)
+    initial.insert(Symbol('x', i), poses[i].compose(delta)); 
+  for (size_t j = 0; j < points.size(); ++j)
+    initial.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
+  
+  // Initialize body_T_sensor wrongly (because we do not know!)
+  initial.insert<Pose3>(Symbol('T',0), Pose3()); 
+
+  std::cout << "initial error: " << graph.error(initial) << std::endl;
+  Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
+  std::cout << "final error: " << graph.error(result) << std::endl;
+
+  initial.at<Pose3>(Symbol('T',0)).print("\nInitial estimate body_T_sensor\n"); /* initial sensor_P_body estimate */
+  result.at<Pose3>(Symbol('T',0)).print("\nFinal estimate body_T_sensor\n");    /* optimized sensor_P_body estimate */
+  body_T_sensor_gt.print("\nGround truth body_T_sensor\n");                     /* sensor_P_body ground truth */
+
+  return 0;
+}
+/* ************************************************************************* */

examples/SFMdata.h

@@ -16,9 +16,10 @@
  */
 
 /**
- * A structure-from-motion example with landmarks
+ * A structure-from-motion example with landmarks, default function arguments give
  *  - The landmarks form a 10 meter cube
  *  - The robot rotates around the landmarks, always facing towards the cube
+ * Passing function argument allows to specificy an initial position, a pose increment and step count.
  */
 
 // As this is a full 3D problem, we will use Pose3 variables to represent the camera
@@ -49,20 +50,19 @@ std::vector<gtsam::Point3> createPoints() {
 }
 
 /* ************************************************************************* */
-std::vector<gtsam::Pose3> createPoses() {
-
+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) {
+  
   // Create the set of ground-truth poses
+  // Default values give a circular trajectory, radius 30 at pi/4 intervals, always facing the circle center
   std::vector<gtsam::Pose3> poses;
-  double radius = 30.0;
-  int i = 0;
-  double theta = 0.0;
-  gtsam::Point3 up(0,0,1);
-  gtsam::Point3 target(0,0,0);
-  for(; i < 8; ++i, theta += 2*M_PI/8) {
-    gtsam::Point3 position = gtsam::Point3(radius*cos(theta), radius*sin(theta), 0.0);
-    gtsam::SimpleCamera camera = gtsam::SimpleCamera::Lookat(position, target, up);
-    poses.push_back(camera.pose());
+  int i = 1;
+  poses.push_back(init);
+  for(; i < steps; ++i) {
+    poses.push_back(poses[i-1].compose(delta));
   }
+  
   return poses;
-}
-/* ************************************************************************* */
+}