From bef23a20080538d06e7195dc87cb4ce09ebc778a Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 2 Oct 2014 11:21:24 +0200
Subject: [PATCH] ExpressionExample now only uses BADFactors and yields same
 result as SFMExample

---
 examples/SFMExample.cpp                       |  2 ++
 gtsam_unstable/examples/ExpressionExample.cpp | 31 ++++++++++---------
 2 files changed, 19 insertions(+), 14 deletions(-)

diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp
index 9fa4bd026..207651957 100644
--- a/examples/SFMExample.cpp
+++ b/examples/SFMExample.cpp
@@ -111,6 +111,8 @@ int main(int argc, char* argv[]) {
   /* Optimize the graph and print results */
   Values result = DoglegOptimizer(graph, initialEstimate).optimize();
   result.print("Final results:\n");
+  cout << "initial error = " << graph.error(initialEstimate) << endl;
+  cout << "final error = " << graph.error(result) << endl;
 
   return 0;
 }
diff --git a/gtsam_unstable/examples/ExpressionExample.cpp b/gtsam_unstable/examples/ExpressionExample.cpp
index 00336a319..90de1a146 100644
--- a/gtsam_unstable/examples/ExpressionExample.cpp
+++ b/gtsam_unstable/examples/ExpressionExample.cpp
@@ -44,7 +44,7 @@ using namespace gtsam;
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
 
-  Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
+  Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
   noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
 
   // Create the set of ground-truth landmarks and poses
@@ -60,37 +60,40 @@ int main(int argc, char* argv[]) {
   // Here we don't use a PriorFactor but directly the BADFactor class
   // The object x0 is an Expression, and we create a factor wanting it to be equal to poses[0]
   Pose3_ x0('x',0);
-//  graph.push_back(BADFactor<Pose3>(poses[0], x0, poseNoise));
-  graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
+  graph.push_back(BADFactor<Pose3>(poseNoise, poses[0], x0));
+
+  // We create a constant Expression for the calibration here
+  Cal3_S2_ cK(K);
 
   // Simulated measurements from each camera pose, adding them to the factor graph
   for (size_t i = 0; i < poses.size(); ++i) {
     for (size_t j = 0; j < points.size(); ++j) {
-      SimpleCamera camera(poses[i], *K);
+      SimpleCamera camera(poses[i], K);
       Point2 measurement = camera.project(points[j]);
-      graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
+      // Below an expression for the prediction of the measurement:
+      Pose3_ x('x', i);
+      Point3_ p('l', j);
+      Expression<Point2> prediction = uncalibrate(cK, project(transform_to(x, p)));
+      // Again, here we use a BADFactor
+      graph.push_back(BADFactor<Point2>(measurementNoise, measurement, prediction));
     }
   }
 
-  // Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
-  // Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
-  // between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
+  // Add prior on first point to constrain scale, again with BADFActor
   noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
-  graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
-  graph.print("Factor Graph:\n");
+  graph.push_back(BADFactor<Point3>(pointNoise, points[0], Point3_('l', 0)));
 
-  // Create the data structure to hold the initial estimate to the solution
-  // Intentionally initialize the variables off from the ground truth
+  // Create perturbed initial
   Values initialEstimate;
   for (size_t i = 0; i < poses.size(); ++i)
     initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
   for (size_t j = 0; j < points.size(); ++j)
     initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
-  initialEstimate.print("Initial Estimates:\n");
+  cout << "initial error = " << graph.error(initialEstimate) << endl;
 
   /* Optimize the graph and print results */
   Values result = DoglegOptimizer(graph, initialEstimate).optimize();
-  result.print("Final results:\n");
+  cout << "final error = " << graph.error(result) << endl;
 
   return 0;
 }