Keys for poses/cameras are naked

gtsam/sfm/SfmData.cpp

@@ -29,7 +29,6 @@ using std::cout;
 using std::endl;
 
 using gtsam::symbol_shorthand::P;
-using gtsam::symbol_shorthand::X;
 
 /* ************************************************************************** */
 void SfmData::print(const std::string &s) const {
@@ -356,7 +355,7 @@ bool writeBALfromValues(const std::string &filename, const SfmData &data,
   size_t nrPoses = values.count<Pose3>();
   if (nrPoses == dataValues.cameras.size()) {  // we only estimated camera poses
     for (size_t i = 0; i < dataValues.cameras.size(); i++) {  // for each camera
-      Pose3 pose = values.at<Pose3>(X(i));
+      Pose3 pose = values.at<Pose3>(i);
       Cal3Bundler K = dataValues.cameras[i].calibration();
       Camera camera(pose, K);
       dataValues.cameras[i] = camera;
@@ -415,8 +414,7 @@ NonlinearFactorGraph SfmData::generalSfmFactors(
     for (const SfmMeasurement &m : track.measurements) {
       size_t i = m.first;
       Point2 uv = m.second;
-      factors.emplace_shared<ProjectionFactor>(
+      factors.emplace_shared<ProjectionFactor>(uv, model, i, P(j));
-          uv, model, X(i), P(j));  // note use of shorthand symbols X and P
     }
     j += 1;
   }
@@ -432,7 +430,7 @@ NonlinearFactorGraph SfmData::sfmFactorGraph(
   NonlinearFactorGraph graph = generalSfmFactors(model);
   using noiseModel::Constrained;
   if (fixedCamera) {
-    graph.addPrior(X(*fixedCamera), cameras[0], Constrained::All(9));
+    graph.addPrior(*fixedCamera, cameras[0], Constrained::All(9));
   }
   if (fixedPoint) {
     graph.addPrior(P(*fixedPoint), tracks[0].p, Constrained::All(3));
@@ -452,7 +450,7 @@ Values initialCamerasEstimate(const SfmData &db) {
 Values initialCamerasAndPointsEstimate(const SfmData &db) {
   Values initial;
   size_t i = 0, j = 0;
-  for (const SfmCamera &camera : db.cameras) initial.insert((i++), camera);
+  for (const SfmCamera &camera : db.cameras) initial.insert(i++, camera);
   for (const SfmTrack &track : db.tracks) initial.insert(P(j++), track.p);
   return initial;
 }

gtsam/sfm/SfmData.h

@@ -83,7 +83,7 @@ struct GTSAM_EXPORT SfmData {
   SfmCamera camera(size_t idx) const { return cameras[idx]; }
 
   /**
-   * @brief Create projection factors using keys X(i) and P(j)
+   * @brief Create projection factors using keys i and P(j)
    *
    * @param model a noise model for projection errors
    * @return NonlinearFactorGraph
@@ -95,6 +95,8 @@ struct GTSAM_EXPORT SfmData {
   /**
    * @brief Create factor graph with projection factors and gauge fix.
    *
+   * Note: pose keys are simply integer indices, points use Symbol('p', j).
+   *
    * @param model a noise model for projection errors
    * @param fixedCamera which camera to fix, if any (use boost::none if none)
    * @param fixedPoint which point to fix, if any (use boost::none if none)
@@ -177,8 +179,8 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, const SfmData& data);
  * @param data SfM structure where the data is stored
  * @param values structure where the graph values are stored (values can be
  * either Pose3 or PinholeCamera<Cal3Bundler> for the cameras, and should be
- * Point3 for the 3D points). Note that the current version assumes that the
+ * Point3 for the 3D points). Note: assumes that the keys are "i" for pose i
- * keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
+ * and "Symbol::('p',j)" for landmark j.
  * @return true if the parsing was successful, false otherwise
  */
 GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
@@ -212,7 +214,10 @@ GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);
 GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
 
 /**
- * @brief This function creates initial values for cameras from db
+ * @brief This function creates initial values for cameras from db.
+ *
+ * No symbol is used, so camera keys are simply integer indices.
+ *
  * @param SfmData
  * @return Values
  */
@@ -220,6 +225,9 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
 
 /**
  * @brief This function creates initial values for cameras and points from db
+ *
+ * Note: Pose keys are simply integer indices, points use Symbol('p', j).
+ *
  * @param SfmData
  * @return Values
  */

gtsam/sfm/tests/testSfmData.cpp

@@ -25,7 +25,6 @@ using namespace std;
 using namespace gtsam;
 
 using gtsam::symbol_shorthand::P;
-using gtsam::symbol_shorthand::X;
 
 namespace gtsam {
 GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
@@ -167,19 +166,19 @@ TEST(dataSet, writeBALfromValues_Dubrovnik) {
   Pose3 poseChange =
       Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.3, 0.1, 0.3));
 
-  Values value;
+  Values values;
   for (size_t i = 0; i < readData.numberCameras(); i++) {  // for each camera
     Pose3 pose = poseChange.compose(readData.cameras[i].pose());
-    value.insert(X(i), pose);
+    values.insert(i, pose);
   }
   for (size_t j = 0; j < readData.numberTracks(); j++) {  // for each point
     Point3 point = poseChange.transformFrom(readData.tracks[j].p);
-    value.insert(P(j), point);
+    values.insert(P(j), point);
   }
 
   // Write values and readData to a file
   const string filenameToWrite = createRewrittenFileName(filenameToRead);
-  writeBALfromValues(filenameToWrite, readData, value);
+  writeBALfromValues(filenameToWrite, readData, values);
 
   // Read the file we wrote
   SfmData writtenData = SfmData::FromBalFile(filenameToWrite);
@@ -199,11 +198,11 @@ TEST(dataSet, writeBALfromValues_Dubrovnik) {
   EXPECT(assert_equal(expected, actual, 12));
 
   Pose3 expectedPose = camera0.pose();
-  Pose3 actualPose = value.at<Pose3>(X(0));
+  Pose3 actualPose = values.at<Pose3>(0);
   EXPECT(assert_equal(expectedPose, actualPose, 1e-7));
 
   Point3 expectedPoint = track0.p;
-  Point3 actualPoint = value.at<Point3>(P(0));
+  Point3 actualPoint = values.at<Point3>(P(0));
   EXPECT(assert_equal(expectedPoint, actualPoint, 1e-6));
 }
 

python/gtsam/examples/SFMExample_bal.py

@@ -17,7 +17,7 @@ import sys
 import gtsam
 from gtsam import (GeneralSFMFactorCal3Bundler, SfmData,
                    PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3)
-from gtsam.symbol_shorthand import C, P  # type: ignore
+from gtsam.symbol_shorthand import P  # type: ignore
 from gtsam.utils import plot  # type: ignore
 from matplotlib import pyplot as plt
 
@@ -29,9 +29,8 @@ DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre"
 def plot_scene(scene_data: SfmData, result: gtsam.Values) -> None:
     """Plot the SFM results."""
     plot_vals = gtsam.Values()
-    for cam_idx in range(scene_data.numberCameras()):
+    for i in range(scene_data.numberCameras()):
-        plot_vals.insert(C(cam_idx),
+        plot_vals.insert(i, result.atPinholeCameraCal3Bundler(i).pose())
-                         result.atPinholeCameraCal3Bundler(C(cam_idx)).pose())
     for j in range(scene_data.numberTracks()):
         plot_vals.insert(P(j), result.atPoint3(P(j)))
 
@@ -64,12 +63,12 @@ def run(args: argparse.Namespace) -> None:
             # i represents the camera index, and uv is the 2d measurement
             i, uv = track.measurement(m_idx)
             # note use of shorthand symbols C and P
-            graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j)))
+            graph.add(GeneralSFMFactorCal3Bundler(uv, noise, i, P(j)))
 
     # Add a prior on pose x1. This indirectly specifies where the origin is.
     graph.push_back(
         PriorFactorPinholeCameraCal3Bundler(
-            C(0), scene_data.camera(0),
+            0, scene_data.camera(0),
             gtsam.noiseModel.Isotropic.Sigma(9, 0.1)))
     # Also add a prior on the position of the first landmark to fix the scale
     graph.push_back(
@@ -82,9 +81,9 @@ def run(args: argparse.Namespace) -> None:
 
     i = 0
     # add each PinholeCameraCal3Bundler
-    for cam_idx in range(scene_data.numberCameras()):
+    for i in range(scene_data.numberCameras()):
-        camera = scene_data.camera(cam_idx)
+        camera = scene_data.camera(i)
-        initial.insert(C(i), camera)
+        initial.insert(i, camera)
         i += 1
 
     # add each SfmTrack