From 7adc9898adb2fca488b07d94f7c720da5bb64ce0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Jan 2022 16:22:26 -0500 Subject: [PATCH] Keys for poses/cameras are naked --- gtsam/sfm/SfmData.cpp | 10 ++++------ gtsam/sfm/SfmData.h | 16 ++++++++++++---- gtsam/sfm/tests/testSfmData.cpp | 13 ++++++------- python/gtsam/examples/SFMExample_bal.py | 17 ++++++++--------- 4 files changed, 30 insertions(+), 26 deletions(-) diff --git a/gtsam/sfm/SfmData.cpp b/gtsam/sfm/SfmData.cpp index 5c76b076a..43b1f5911 100644 --- a/gtsam/sfm/SfmData.cpp +++ b/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(); 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(X(i)); + Pose3 pose = values.at(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( - uv, model, X(i), P(j)); // note use of shorthand symbols X and P + factors.emplace_shared(uv, model, i, P(j)); } 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; } diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h index def20807a..afce12205 100644 --- a/gtsam/sfm/SfmData.h +++ b/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 for the cameras, and should be - * Point3 for the 3D points). Note that the current version assumes that the - * keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1 + * Point3 for the 3D points). Note: assumes that the keys are "i" for pose i + * 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 */ diff --git a/gtsam/sfm/tests/testSfmData.cpp b/gtsam/sfm/tests/testSfmData.cpp index a48e3e7f3..7bd5d27e7 100644 --- a/gtsam/sfm/tests/testSfmData.cpp +++ b/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(X(0)); + Pose3 actualPose = values.at(0); EXPECT(assert_equal(expectedPose, actualPose, 1e-7)); Point3 expectedPoint = track0.p; - Point3 actualPoint = value.at(P(0)); + Point3 actualPoint = values.at(P(0)); EXPECT(assert_equal(expectedPoint, actualPoint, 1e-6)); } diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 415436157..3d71590a9 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/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()): - plot_vals.insert(C(cam_idx), - result.atPinholeCameraCal3Bundler(C(cam_idx)).pose()) + for i in range(scene_data.numberCameras()): + plot_vals.insert(i, result.atPinholeCameraCal3Bundler(i).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()): - camera = scene_data.camera(cam_idx) - initial.insert(C(i), camera) + for i in range(scene_data.numberCameras()): + camera = scene_data.camera(i) + initial.insert(i, camera) i += 1 # add each SfmTrack