changed the SFMdata functions so that it allows the passage of function arguments to generate a trajectory; default arguments result in the original behaviour (described in header). In the range bearing examples: fixed weirdo text-artifacts, add newline for readability, added underscore the prediction expression.

examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp

@@ -11,86 +11,65 @@
 #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;
 
-// These functions are very similar to those in the SFM example, except that you can give it a fixed delta in between poses for n steps.
-/* ************************************************************************* */
-std::vector<Point3> createPoints() {
-
-  // Create the set of ground-truth landmarks
-  std::vector<Point3> points;
-  points.push_back(Point3(10.0,10.0,10.0));
-  points.push_back(Point3(-10.0,10.0,10.0));
-  points.push_back(Point3(-10.0,-10.0,10.0));
-  points.push_back(Point3(10.0,-10.0,10.0));
-  points.push_back(Point3(10.0,10.0,-10.0));
-  points.push_back(Point3(-10.0,10.0,-10.0));
-  points.push_back(Point3(-10.0,-10.0,-10.0));
-  points.push_back(Point3(10.0,-10.0,-10.0));
-
-  return points;
-}
-
-/* ************************************************************************* */
-std::vector<Pose3> createPoses(Pose3 delta, int steps) {
-
-  // Create the set of ground-truth poses
-  std::vector<Pose3> poses;
-  Pose3 pose = Pose3();
-  int i = 0;
-  for(; i < steps; ++i) {
-    poses.push_back(pose);
-    pose = pose.compose(delta);
-  }
-  poses.push_back(pose);
-
-  return poses;
-}
-
 /* ************************************************************************* */
 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(delta_pose1, steps);
-  auto poses2 = createPoses(delta_pose2, steps);
-  auto poses3 = createPoses(delta_pose3, steps);
+  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
+  // 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));
-  // a graph
+  
+  // 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());ExpressiExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampon examp
+  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);nice
+  Pose3_ x_('x', 0);
   Pose3_ body_T_sensor_('T', 0);
-  // add a prior on the body-pose.
+  
+  // 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) {
-      // Create the expression
-      auto prediction = Expression<BearingRange3D>( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j));
-      // Create a *perfect* measurementExpression exampExpreExpression exampExpression exampExpression exampssion examp
+      
+      // 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);
+      graph.addExpressionFactor(prediction_, measurement, bearingRangeNoise);
     }
+    
     // and add a between factor to the graph
     if (i > 0)
     {
@@ -107,7 +86,7 @@ int main(int argc, char* argv[]) {
   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!)
+  // 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;

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;
-}
-/* ************************************************************************* */
+}