Two new scripts with expressions

gtsam/slam/expressions.h

@@ -36,6 +36,10 @@ inline Point3_ transform_to(const Pose3_& x, const Point3_& p) {
   return Point3_(x, &Pose3::transform_to, p);
 }
 
+inline Point3_ transform_from(const Pose3_& x, const Point3_& p) {
+  return Point3_(x, &Pose3::transform_from, p);
+}
+
 // Projection
 
 typedef Expression<Cal3_S2> Cal3_S2_;

timing/timeSFMBAL.h

@@ -41,7 +41,7 @@ static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2);
 SfM_data preamble(int argc, char* argv[]) {
   // primitive argument parsing:
   if (argc > 2) {
-    if (string(argv[1]) == "--colamd")
+    if (strcmp(argv[1], "--colamd"))
       gUseSchur = false;
     else
       throw runtime_error("Usage: timeSFMBALxxx [--colamd] [BALfile]");
@@ -50,14 +50,14 @@ SfM_data preamble(int argc, char* argv[]) {
   // Load BAL file
   SfM_data db;
   string defaultFilename = findExampleDataFile("dubrovnik-16-22106-pre");
-  bool success = readBAL(argc > 1 ? argv[argc] : defaultFilename, db);
+  bool success = readBAL(argc > 1 ? argv[argc - 1] : defaultFilename, db);
   if (!success) throw runtime_error("Could not access file!");
   return db;
 }
 
 // Create ordering and optimize
 int optimize(const SfM_data& db, const NonlinearFactorGraph& graph,
-             const Values& initial) {
+             const Values& initial, bool separateCalibration = false) {
   using symbol_shorthand::P;
 
   // Set parameters to be similar to ceres
@@ -69,7 +69,10 @@ int optimize(const SfM_data& db, const NonlinearFactorGraph& graph,
     // Create Schur-complement ordering
     Ordering ordering;
     for (size_t j = 0; j < db.number_tracks(); j++) ordering.push_back(P(j));
-    for (size_t i = 0; i < db.number_cameras(); i++) ordering.push_back(C(i));
+    for (size_t i = 0; i < db.number_cameras(); i++) {
+      ordering.push_back(C(i));
+      if (separateCalibration) ordering.push_back(K(i));
+    }
     params.setOrdering(ordering);
   }
 

timing/timeSFMBALcamTnav.cpp

@@ -0,0 +1,63 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    timeSFMBALcamTnav.cpp
+ * @brief   time SFM with BAL file, expressions with camTnav pose
+ * @author  Frank Dellaert
+ * @date    July 5, 2015
+ */
+
+#include "timeSFMBAL.h"
+
+#include <gtsam/slam/expressions.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Point3.h>
+
+#include <boost/foreach.hpp>
+
+using namespace std;
+using namespace gtsam;
+
+int main(int argc, char* argv[]) {
+  // parse options and read BAL file
+  SfM_data db = preamble(argc, argv);
+
+  // Build graph using conventional GeneralSFMFactor
+  NonlinearFactorGraph graph;
+  for (size_t j = 0; j < db.number_tracks(); j++) {
+    BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) {
+      size_t i = m.first;
+      Point2 z = m.second;
+      Pose3_ camTnav_(C(i));
+      Cal3Bundler_ calibration_(K(i));
+      Point3_ nav_point_(P(j));
+      graph.addExpressionFactor(
+          gNoiseModel, z,
+          uncalibrate(calibration_,  // now using transform_from !!!:
+                      project(transform_from(camTnav_, nav_point_))));
+    }
+  }
+
+  Values initial;
+  size_t i = 0, j = 0;
+  BOOST_FOREACH (const SfM_Camera& camera, db.cameras) {
+    initial.insert(C(i), camera.pose().inverse());  // inverse !!!
+    initial.insert(K(i), camera.calibration());
+    i += 1;
+  }
+  BOOST_FOREACH (const SfM_Track& track, db.tracks)
+    initial.insert(P(j++), track.p);
+
+  bool separateCalibration = true;
+  return optimize(db, graph, initial, separateCalibration);
+}

timing/timeSFMBALnavTcam.cpp

@@ -0,0 +1,63 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    timeSFMBALnavTcam.cpp
+ * @brief   time SFM with BAL file, expressions with navTcam pose
+ * @author  Frank Dellaert
+ * @date    July 5, 2015
+ */
+
+#include "timeSFMBAL.h"
+
+#include <gtsam/slam/expressions.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Point3.h>
+
+#include <boost/foreach.hpp>
+
+using namespace std;
+using namespace gtsam;
+
+int main(int argc, char* argv[]) {
+  // parse options and read BAL file
+  SfM_data db = preamble(argc, argv);
+
+  // Build graph using conventional GeneralSFMFactor
+  NonlinearFactorGraph graph;
+  for (size_t j = 0; j < db.number_tracks(); j++) {
+    BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) {
+      size_t i = m.first;
+      Point2 z = m.second;
+      Pose3_ navTcam_(C(i));
+      Cal3Bundler_ calibration_(K(i));
+      Point3_ nav_point_(P(j));
+      graph.addExpressionFactor(
+          gNoiseModel, z,
+          uncalibrate(calibration_,
+                      project(transform_to(navTcam_, nav_point_))));
+    }
+  }
+
+  Values initial;
+  size_t i = 0, j = 0;
+  BOOST_FOREACH (const SfM_Camera& camera, db.cameras) {
+    initial.insert(C(i), camera.pose());
+    initial.insert(K(i), camera.calibration());
+    i += 1;
+  }
+  BOOST_FOREACH (const SfM_Track& track, db.tracks)
+    initial.insert(P(j++), track.p);
+
+  bool separateCalibration = true;
+  return optimize(db, graph, initial, separateCalibration);
+}