From cd94e7dda2c7279e1d53ff19b6e748742071fbec Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Jan 2022 07:23:36 -0500 Subject: [PATCH 01/10] fix python example --- python/gtsam/examples/SFMExample.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/python/gtsam/examples/SFMExample.py b/python/gtsam/examples/SFMExample.py index f0c4c82bae..87bb3cb871 100644 --- a/python/gtsam/examples/SFMExample.py +++ b/python/gtsam/examples/SFMExample.py @@ -8,7 +8,6 @@ A structure-from-motion problem on a simulated dataset """ -from __future__ import print_function import gtsam import matplotlib.pyplot as plt @@ -89,7 +88,7 @@ def main(): point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) factor = PriorFactorPoint3(L(0), points[0], point_noise) graph.push_back(factor) - graph.print_('Factor Graph:\n') + graph.print('Factor Graph:\n') # Create the data structure to hold the initial estimate to the solution # Intentionally initialize the variables off from the ground truth @@ -100,7 +99,7 @@ def main(): for j, point in enumerate(points): transformed_point = point + 0.1*np.random.randn(3) initial_estimate.insert(L(j), transformed_point) - initial_estimate.print_('Initial Estimates:\n') + initial_estimate.print('Initial Estimates:\n') # Optimize the graph and print results params = gtsam.DoglegParams() @@ -108,7 +107,7 @@ def main(): optimizer = DoglegOptimizer(graph, initial_estimate, params) print('Optimizing:') result = optimizer.optimize() - result.print_('Final results:\n') + result.print('Final results:\n') print('initial error = {}'.format(graph.error(initial_estimate))) print('final error = {}'.format(graph.error(result))) From 0fd49efabae89457500948bcae54560d834e44c9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Jan 2022 08:32:13 -0500 Subject: [PATCH 02/10] Moved Sfm classes to their own files --- gtsam/sfm/SfmData.cpp | 51 +++++++++++ gtsam/sfm/SfmData.h | 103 ++++++++++++++++++++++ gtsam/sfm/SfmTrack.cpp | 71 +++++++++++++++ gtsam/sfm/SfmTrack.h | 133 ++++++++++++++++++++++++++++ gtsam/slam/dataset.cpp | 24 +++--- gtsam/slam/dataset.h | 191 +---------------------------------------- 6 files changed, 371 insertions(+), 202 deletions(-) create mode 100644 gtsam/sfm/SfmData.cpp create mode 100644 gtsam/sfm/SfmData.h create mode 100644 gtsam/sfm/SfmTrack.cpp create mode 100644 gtsam/sfm/SfmTrack.h diff --git a/gtsam/sfm/SfmData.cpp b/gtsam/sfm/SfmData.cpp new file mode 100644 index 0000000000..715dda2ed4 --- /dev/null +++ b/gtsam/sfm/SfmData.cpp @@ -0,0 +1,51 @@ +/* ---------------------------------------------------------------------------- + + * 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 SfmData.cpp + * @date January 2022 + * @author Frank dellaert + * @brief Data structure for dealing with Structure from Motion data + */ + +#include + +namespace gtsam { + +void SfmData::print(const std::string& s) const { + std::cout << "Number of cameras = " << nrCameras() << std::endl; + std::cout << "Number of tracks = " << nrTracks() << std::endl; +} + +bool SfmData::equals(const SfmData& sfmData, double tol) const { + // check number of cameras and tracks + if (nrCameras() != sfmData.nrCameras() || nrTracks() != sfmData.nrTracks()) { + return false; + } + + // check each camera + for (size_t i = 0; i < nrCameras(); ++i) { + if (!camera(i).equals(sfmData.camera(i), tol)) { + return false; + } + } + + // check each track + for (size_t j = 0; j < nrTracks(); ++j) { + if (!track(j).equals(sfmData.track(j), tol)) { + return false; + } + } + + return true; +} + +} // namespace gtsam diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h new file mode 100644 index 0000000000..91b7a5d1ee --- /dev/null +++ b/gtsam/sfm/SfmData.h @@ -0,0 +1,103 @@ +/* ---------------------------------------------------------------------------- + + * 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 SfmData.h + * @date January 2022 + * @author Frank dellaert + * @brief Data structure for dealing with Structure from Motion data + */ + +#pragma once + +#include +#include +#include + +#include +#include + +namespace gtsam { + +/// Define the structure for the camera poses +typedef PinholeCamera SfmCamera; + +/** + * @brief SfmData stores a bunch of SfmTracks + * @addtogroup sfm + */ +struct SfmData { + std::vector cameras; ///< Set of cameras + + std::vector tracks; ///< Sparse set of points + + /// @name Standard Interface + /// @{ + + /// Add a track to SfmData + void addTrack(const SfmTrack& t) { tracks.push_back(t); } + + /// Add a camera to SfmData + void addCamera(const SfmCamera& cam) { cameras.push_back(cam); } + + /// The number of reconstructed 3D points + size_t nrTracks() const { return tracks.size(); } + + /// The number of cameras + size_t nrCameras() const { return cameras.size(); } + + /// The track formed by series of landmark measurements + SfmTrack track(size_t idx) const { return tracks[idx]; } + + /// The camera pose at frame index `idx` + SfmCamera camera(size_t idx) const { return cameras[idx]; } + + /// @} + /// @name Testable + /// @{ + + /// print + void print(const std::string& s = "") const; + + /// assert equality up to a tolerance + bool equals(const SfmData& sfmData, double tol = 1e-9) const; + + /// @} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /// @name Deprecated + /// @{ + void GTSAM_DEPRECATED add_track(const SfmTrack& t) { tracks.push_back(t); } + void GTSAM_DEPRECATED add_camera(const SfmCamera& cam) { + cameras.push_back(cam); + } + size_t GTSAM_DEPRECATED number_tracks() const { return tracks.size(); } + size_t GTSAM_DEPRECATED number_cameras() const { return cameras.size(); } + /// @} +#endif + /// @name Serialization + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(cameras); + ar& BOOST_SERIALIZATION_NVP(tracks); + } + + /// @} +}; + +/// traits +template <> +struct traits : public Testable {}; + +} // namespace gtsam diff --git a/gtsam/sfm/SfmTrack.cpp b/gtsam/sfm/SfmTrack.cpp new file mode 100644 index 0000000000..11f104c09e --- /dev/null +++ b/gtsam/sfm/SfmTrack.cpp @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + + * 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 SfmTrack.cpp + * @date January 2022 + * @author Frank Dellaert + * @brief A simple data structure for a track in Structure from Motion + */ + +#include + +#include + +namespace gtsam { + +void SfmTrack::print(const std::string& s) const { + std::cout << "Track with " << measurements.size(); + std::cout << " measurements of point " << p << std::endl; +} + +bool SfmTrack::equals(const SfmTrack& sfmTrack, double tolÏ) const { + // check the 3D point + if (!p.isApprox(sfmTrack.p)) { + return false; + } + + // check the RGB values + if (r != sfmTrack.r || g != sfmTrack.g || b != sfmTrack.b) { + return false; + } + + // compare size of vectors for measurements and siftIndices + if (nrMeasurements() != sfmTrack.nrMeasurements() || + siftIndices.size() != sfmTrack.siftIndices.size()) { + return false; + } + + // compare measurements (order sensitive) + for (size_t idx = 0; idx < nrMeasurements(); ++idx) { + SfmMeasurement measurement = measurements[idx]; + SfmMeasurement otherMeasurement = sfmTrack.measurements[idx]; + + if (measurement.first != otherMeasurement.first || + !measurement.second.isApprox(otherMeasurement.second)) { + return false; + } + } + + // compare sift indices (order sensitive) + for (size_t idx = 0; idx < siftIndices.size(); ++idx) { + SiftIndex index = siftIndices[idx]; + SiftIndex otherIndex = sfmTrack.siftIndices[idx]; + + if (index.first != otherIndex.first || index.second != otherIndex.second) { + return false; + } + } + + return true; +} + +} // namespace gtsam diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h new file mode 100644 index 0000000000..35603a6f36 --- /dev/null +++ b/gtsam/sfm/SfmTrack.h @@ -0,0 +1,133 @@ +/* ---------------------------------------------------------------------------- + + * 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 SfmTrack.h + * @date January 2022 + * @author Frank Dellaert + * @brief A simple data structure for a track in Structure from Motion + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace gtsam { + +/// A measurement with its camera index +typedef std::pair SfmMeasurement; + +/// Sift index for SfmTrack +typedef std::pair SiftIndex; + +/** + * @brief An SfmTrack stores SfM measurements grouped in a track + * @addtogroup sfm + */ +struct SfmTrack { + Point3 p; ///< 3D position of the point + float r, g, b; ///< RGB color of the 3D point + + /// The 2D image projections (id,(u,v)) + std::vector measurements; + + /// The feature descriptors + std::vector siftIndices; + + /// @name Constructors + /// @{ + + explicit SfmTrack(float r = 0, float g = 0, float b = 0) + : p(0, 0, 0), r(r), g(g), b(b) {} + + explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0, + float b = 0) + : p(pt), r(r), g(g), b(b) {} + + /// @} + /// @name Standard Interface + /// @{ + + /// Add measurement (camera_idx, Point2) to track + void addMeasurement(size_t idx, const gtsam::Point2& m) { + measurements.emplace_back(idx, m); + } + + /// Total number of measurements in this track + size_t nrMeasurements() const { return measurements.size(); } + + /// Get the measurement (camera index, Point2) at pose index `idx` + const SfmMeasurement& measurement(size_t idx) const { + return measurements[idx]; + } + + /// Get the SIFT feature index corresponding to the measurement at `idx` + const SiftIndex& siftIndex(size_t idx) const { return siftIndices[idx]; } + + /// Get 3D point + const Point3& point3() const { return p; } + + /// Get RGB values describing 3d point + Point3 rgb() const { return Point3(r, g, b); } + + /// @} + /// @name Testable + /// @{ + + /// print + void print(const std::string& s = "") const; + + /// assert equality up to a tolerance + bool equals(const SfmTrack& sfmTrack, double tol = 1e-9) const; + + /// @} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /// @name Deprecated + /// @{ + void GTSAM_DEPRECATED add_measurement(size_t idx, const gtsam::Point2& m) { + measurements.emplace_back(idx, m); + } + + size_t GTSAM_DEPRECATED number_measurements() const { + return measurements.size(); + } + /// @} +#endif + /// @name Serialization + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(p); + ar& BOOST_SERIALIZATION_NVP(r); + ar& BOOST_SERIALIZATION_NVP(g); + ar& BOOST_SERIALIZATION_NVP(b); + ar& BOOST_SERIALIZATION_NVP(measurements); + ar& BOOST_SERIALIZATION_NVP(siftIndices); + } + /// @} +}; + +template +struct traits; + +template <> +struct traits : public Testable {}; + +} // namespace gtsam diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index fc020b05b5..8acdbfc14a 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1149,19 +1149,19 @@ bool writeBAL(const string &filename, SfmData &data) { // Write the number of camera poses and 3D points size_t nrObservations = 0; - for (size_t j = 0; j < data.number_tracks(); j++) { - nrObservations += data.tracks[j].number_measurements(); + for (size_t j = 0; j < data.nrTracks(); j++) { + nrObservations += data.tracks[j].nrMeasurements(); } // Write observations - os << data.number_cameras() << " " << data.number_tracks() << " " + os << data.nrCameras() << " " << data.nrTracks() << " " << nrObservations << endl; os << endl; - for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j + for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j const SfmTrack &track = data.tracks[j]; - for (size_t k = 0; k < track.number_measurements(); + for (size_t k = 0; k < track.nrMeasurements(); k++) { // for each observation of the 3D point j size_t i = track.measurements[k].first; // camera id double u0 = data.cameras[i].calibration().px(); @@ -1186,7 +1186,7 @@ bool writeBAL(const string &filename, SfmData &data) { os << endl; // Write cameras - for (size_t i = 0; i < data.number_cameras(); i++) { // for each camera + for (size_t i = 0; i < data.nrCameras(); i++) { // for each camera Pose3 poseGTSAM = data.cameras[i].pose(); Cal3Bundler cameraCalibration = data.cameras[i].calibration(); Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); @@ -1201,7 +1201,7 @@ bool writeBAL(const string &filename, SfmData &data) { } // Write the points - for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j + for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j Point3 point = data.tracks[j].p; os << point.x() << endl; os << point.y() << endl; @@ -1221,8 +1221,8 @@ bool writeBALfromValues(const string &filename, const SfmData &data, // Store poses or cameras in SfmData size_t nrPoses = values.count(); if (nrPoses == - dataValues.number_cameras()) { // we only estimated camera poses - for (size_t i = 0; i < dataValues.number_cameras(); + dataValues.nrCameras()) { // we only estimated camera poses + for (size_t i = 0; i < dataValues.nrCameras(); i++) { // for each camera Pose3 pose = values.at(X(i)); Cal3Bundler K = dataValues.cameras[i].calibration(); @@ -1231,7 +1231,7 @@ bool writeBALfromValues(const string &filename, const SfmData &data, } } else { size_t nrCameras = values.count(); - if (nrCameras == dataValues.number_cameras()) { // we only estimated camera + if (nrCameras == dataValues.nrCameras()) { // we only estimated camera // poses and calibration for (size_t i = 0; i < nrCameras; i++) { // for each camera Key cameraKey = i; // symbol('c',i); @@ -1241,7 +1241,7 @@ bool writeBALfromValues(const string &filename, const SfmData &data, } else { cout << "writeBALfromValues: different number of cameras in " "SfM_dataValues (#cameras " - << dataValues.number_cameras() << ") and values (#cameras " + << dataValues.nrCameras() << ") and values (#cameras " << nrPoses << ", #poses " << nrCameras << ")!!" << endl; return false; } @@ -1249,7 +1249,7 @@ bool writeBALfromValues(const string &filename, const SfmData &data, // Store 3D points in SfmData size_t nrPoints = values.count(), - nrTracks = dataValues.number_tracks(); + nrTracks = dataValues.nrTracks(); if (nrPoints != nrTracks) { cout << "writeBALfromValues: different number of points in " "SfM_dataValues (#points= " diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 1611d78f65..d5c93c7f83 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -208,196 +209,6 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, /// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); -/// A measurement with its camera index -typedef std::pair SfmMeasurement; - -/// Sift index for SfmTrack -typedef std::pair SiftIndex; - -/// Define the structure for the 3D points -struct SfmTrack { - SfmTrack(float r = 0, float g = 0, float b = 0): p(0,0,0), r(r), g(g), b(b) {} - SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0, float b = 0) : p(pt), r(r), g(g), b(b) {} - - Point3 p; ///< 3D position of the point - float r, g, b; ///< RGB color of the 3D point - std::vector measurements; ///< The 2D image projections (id,(u,v)) - std::vector siftIndices; - - /// Get RGB values describing 3d point - const Point3 rgb() const { return Point3(r, g, b); } - - /// Total number of measurements in this track - size_t number_measurements() const { - return measurements.size(); - } - /// Get the measurement (camera index, Point2) at pose index `idx` - SfmMeasurement measurement(size_t idx) const { - return measurements[idx]; - } - /// Get the SIFT feature index corresponding to the measurement at `idx` - SiftIndex siftIndex(size_t idx) const { - return siftIndices[idx]; - } - /// Get 3D point - const Point3& point3() const { - return p; - } - /// Add measurement (camera_idx, Point2) to track - void add_measurement(size_t idx, const gtsam::Point2& m) { - measurements.emplace_back(idx, m); - } - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(p); - ar & BOOST_SERIALIZATION_NVP(r); - ar & BOOST_SERIALIZATION_NVP(g); - ar & BOOST_SERIALIZATION_NVP(b); - ar & BOOST_SERIALIZATION_NVP(measurements); - ar & BOOST_SERIALIZATION_NVP(siftIndices); - } - - /// assert equality up to a tolerance - bool equals(const SfmTrack &sfmTrack, double tol = 1e-9) const { - // check the 3D point - if (!p.isApprox(sfmTrack.p)) { - return false; - } - - // check the RGB values - if (r!=sfmTrack.r || g!=sfmTrack.g || b!=sfmTrack.b) { - return false; - } - - // compare size of vectors for measurements and siftIndices - if (number_measurements() != sfmTrack.number_measurements() || - siftIndices.size() != sfmTrack.siftIndices.size()) { - return false; - } - - // compare measurements (order sensitive) - for (size_t idx = 0; idx < number_measurements(); ++idx) { - SfmMeasurement measurement = measurements[idx]; - SfmMeasurement otherMeasurement = sfmTrack.measurements[idx]; - - if (measurement.first != otherMeasurement.first || - !measurement.second.isApprox(otherMeasurement.second)) { - return false; - } - } - - // compare sift indices (order sensitive) - for (size_t idx = 0; idx < siftIndices.size(); ++idx) { - SiftIndex index = siftIndices[idx]; - SiftIndex otherIndex = sfmTrack.siftIndices[idx]; - - if (index.first != otherIndex.first || - index.second != otherIndex.second) { - return false; - } - } - - return true; - } - - /// print - void print(const std::string& s = "") const { - std::cout << "Track with " << measurements.size(); - std::cout << " measurements of point " << p << std::endl; - } -}; - -/* ************************************************************************* */ -/// traits -template<> -struct traits : public Testable { -}; - - -/// Define the structure for the camera poses -typedef PinholeCamera SfmCamera; - -/// Define the structure for SfM data -struct SfmData { - std::vector cameras; ///< Set of cameras - std::vector tracks; ///< Sparse set of points - size_t number_cameras() const { - return cameras.size(); - } - /// The number of reconstructed 3D points - size_t number_tracks() const { - return tracks.size(); - } - /// The camera pose at frame index `idx` - SfmCamera camera(size_t idx) const { - return cameras[idx]; - } - /// The track formed by series of landmark measurements - SfmTrack track(size_t idx) const { - return tracks[idx]; - } - /// Add a track to SfmData - void add_track(const SfmTrack& t) { - tracks.push_back(t); - } - /// Add a camera to SfmData - void add_camera(const SfmCamera& cam) { - cameras.push_back(cam); - } - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(cameras); - ar & BOOST_SERIALIZATION_NVP(tracks); - } - - /// @} - /// @name Testable - /// @{ - - /// assert equality up to a tolerance - bool equals(const SfmData &sfmData, double tol = 1e-9) const { - // check number of cameras and tracks - if (number_cameras() != sfmData.number_cameras() || - number_tracks() != sfmData.number_tracks()) { - return false; - } - - // check each camera - for (size_t i = 0; i < number_cameras(); ++i) { - if (!camera(i).equals(sfmData.camera(i), tol)) { - return false; - } - } - - // check each track - for (size_t j = 0; j < number_tracks(); ++j) { - if (!track(j).equals(sfmData.track(j), tol)) { - return false; - } - } - - return true; - } - - /// print - void print(const std::string& s = "") const { - std::cout << "Number of cameras = " << number_cameras() << std::endl; - std::cout << "Number of tracks = " << number_tracks() << std::endl; - } -}; - -/* ************************************************************************* */ -/// traits -template<> -struct traits : public Testable { -}; - /** * @brief This function parses a bundler output file and stores the data into a * SfmData structure From 3d51989f2ee6ab300dfa0786707d7ebd957588b9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Jan 2022 08:32:44 -0500 Subject: [PATCH 03/10] Deprecated incorrectly named methods --- examples/SFMExampleExpressions_bal.cpp | 2 +- examples/SFMExample_bal.cpp | 2 +- examples/SFMExample_bal_COLAMD_METIS.cpp | 4 +-- gtsam/slam/SmartFactorBase.h | 2 +- gtsam/slam/slam.i | 12 +++---- gtsam/slam/tests/testDataset.cpp | 32 +++++++++---------- .../slam/tests/testEssentialMatrixFactor.cpp | 6 ++-- python/gtsam/examples/SFMExample_bal.py | 16 +++++----- python/gtsam/tests/test_SfmData.py | 18 +++++------ python/gtsam/tests/test_pickle.py | 4 +-- tests/testGeneralSFMFactorB.cpp | 2 +- timing/timeSFMBAL.cpp | 2 +- timing/timeSFMBAL.h | 4 +-- timing/timeSFMBALautodiff.cpp | 2 +- timing/timeSFMBALcamTnav.cpp | 2 +- timing/timeSFMBALnavTcam.cpp | 2 +- timing/timeSFMBALsmart.cpp | 2 +- 17 files changed, 57 insertions(+), 57 deletions(-) diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp index 3768ee2a34..4cc6b1185d 100644 --- a/examples/SFMExampleExpressions_bal.cpp +++ b/examples/SFMExampleExpressions_bal.cpp @@ -49,7 +49,7 @@ int main(int argc, char* argv[]) { SfmData mydata; readBAL(filename, mydata); cout << boost::format("read %1% tracks on %2% cameras\n") % - mydata.number_tracks() % mydata.number_cameras(); + mydata.nrTracks() % mydata.nrCameras(); // Create a factor graph ExpressionFactorGraph graph; diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index ffb5b195b9..a93e438508 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -43,7 +43,7 @@ int main (int argc, char* argv[]) { // Load the SfM data from file SfmData mydata; readBAL(filename, mydata); - cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); + cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.nrTracks() % mydata.nrCameras(); // Create a factor graph NonlinearFactorGraph graph; diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index b79a9fa285..90b397b06b 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -48,7 +48,7 @@ int main(int argc, char* argv[]) { SfmData mydata; readBAL(filename, mydata); cout << boost::format("read %1% tracks on %2% cameras\n") % - mydata.number_tracks() % mydata.number_cameras(); + mydata.nrTracks() % mydata.nrCameras(); // Create a factor graph NonlinearFactorGraph graph; @@ -131,7 +131,7 @@ int main(int argc, char* argv[]) { cout << "Time comparison by solving " << filename << " results:" << endl; cout << boost::format("%1% point tracks and %2% cameras\n") % - mydata.number_tracks() % mydata.number_cameras() + mydata.nrTracks() % mydata.nrCameras() << endl; tictoc_print_(); diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 209c1196d5..42a9508481 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -146,7 +146,7 @@ class GTSAM_EXPORT SmartFactorBase: public NonlinearFactor { */ template void add(const SFM_TRACK& trackToAdd) { - for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { + for (size_t k = 0; k < trackToAdd.nrMeasurements(); k++) { this->measured_.push_back(trackToAdd.measurements[k].second); this->keys_.push_back(trackToAdd.measurements[k].first); } diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 2b25a43698..51a5904053 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -220,10 +220,10 @@ class SfmTrack { std::vector> measurements; - size_t number_measurements() const; + size_t nrMeasurements() const; pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; - void add_measurement(size_t idx, const gtsam::Point2& m); + void addMeasurement(size_t idx, const gtsam::Point2& m); // enabling serialization functionality void serialize() const; @@ -234,12 +234,12 @@ class SfmTrack { class SfmData { SfmData(); - size_t number_cameras() const; - size_t number_tracks() const; + size_t nrCameras() const; + size_t nrTracks() const; gtsam::PinholeCamera camera(size_t idx) const; gtsam::SfmTrack track(size_t idx) const; - void add_track(const gtsam::SfmTrack& t); - void add_camera(const gtsam::SfmCamera& cam); + void addTrack(const gtsam::SfmTrack& t); + void addCamera(const gtsam::SfmCamera& cam); // enabling serialization functionality void serialize() const; diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index aad9124c53..b88832ff2c 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -160,10 +160,10 @@ TEST( dataSet, Balbianello) CHECK(readBundler(filename, mydata)); // Check number of things - EXPECT_LONGS_EQUAL(5,mydata.number_cameras()); - EXPECT_LONGS_EQUAL(544,mydata.number_tracks()); + EXPECT_LONGS_EQUAL(5,mydata.nrCameras()); + EXPECT_LONGS_EQUAL(544,mydata.nrTracks()); const SfmTrack& track0 = mydata.tracks[0]; - EXPECT_LONGS_EQUAL(3,track0.number_measurements()); + EXPECT_LONGS_EQUAL(3,track0.nrMeasurements()); // Check projection of a given point EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); @@ -470,10 +470,10 @@ TEST( dataSet, readBAL_Dubrovnik) CHECK(readBAL(filename, mydata)); // Check number of things - EXPECT_LONGS_EQUAL(3,mydata.number_cameras()); - EXPECT_LONGS_EQUAL(7,mydata.number_tracks()); + EXPECT_LONGS_EQUAL(3,mydata.nrCameras()); + EXPECT_LONGS_EQUAL(7,mydata.nrTracks()); const SfmTrack& track0 = mydata.tracks[0]; - EXPECT_LONGS_EQUAL(3,track0.number_measurements()); + EXPECT_LONGS_EQUAL(3,track0.nrMeasurements()); // Check projection of a given point EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); @@ -533,16 +533,16 @@ TEST( dataSet, writeBAL_Dubrovnik) CHECK(readBAL(filenameToWrite, writtenData)); // Check that what we read is the same as what we wrote - EXPECT_LONGS_EQUAL(readData.number_cameras(),writtenData.number_cameras()); - EXPECT_LONGS_EQUAL(readData.number_tracks(),writtenData.number_tracks()); + EXPECT_LONGS_EQUAL(readData.nrCameras(),writtenData.nrCameras()); + EXPECT_LONGS_EQUAL(readData.nrTracks(),writtenData.nrTracks()); - for (size_t i = 0; i < readData.number_cameras(); i++){ + for (size_t i = 0; i < readData.nrCameras(); i++){ PinholeCamera expectedCamera = writtenData.cameras[i]; PinholeCamera actualCamera = readData.cameras[i]; EXPECT(assert_equal(expectedCamera,actualCamera)); } - for (size_t j = 0; j < readData.number_tracks(); j++){ + for (size_t j = 0; j < readData.nrTracks(); j++){ // check point SfmTrack expectedTrack = writtenData.tracks[j]; SfmTrack actualTrack = readData.tracks[j]; @@ -556,7 +556,7 @@ TEST( dataSet, writeBAL_Dubrovnik) EXPECT(assert_equal(expectedRGB,actualRGB)); // check measurements - for (size_t k = 0; k < actualTrack.number_measurements(); k++){ + for (size_t k = 0; k < actualTrack.nrMeasurements(); k++){ EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,actualTrack.measurements[k].first); EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second)); } @@ -575,11 +575,11 @@ 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; - for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera + for(size_t i=0; i < readData.nrCameras(); i++){ // for each camera Pose3 pose = poseChange.compose(readData.cameras[i].pose()); value.insert(X(i), pose); } - for(size_t j=0; j < readData.number_tracks(); j++){ // for each point + for(size_t j=0; j < readData.nrTracks(); j++){ // for each point Point3 point = poseChange.transformFrom( readData.tracks[j].p ); value.insert(P(j), point); } @@ -594,10 +594,10 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ // Check that the reprojection errors are the same and the poses are correct // Check number of things - EXPECT_LONGS_EQUAL(3,writtenData.number_cameras()); - EXPECT_LONGS_EQUAL(7,writtenData.number_tracks()); + EXPECT_LONGS_EQUAL(3,writtenData.nrCameras()); + EXPECT_LONGS_EQUAL(7,writtenData.nrTracks()); const SfmTrack& track0 = writtenData.tracks[0]; - EXPECT_LONGS_EQUAL(3,track0.number_measurements()); + EXPECT_LONGS_EQUAL(3,track0.nrMeasurements()); // Check projection of a given point EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 03775a70f0..6df59ba489 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -632,14 +632,14 @@ TEST(EssentialMatrixFactor2, extraMinimization) { // We start with a factor graph and add constraints to it // Noise sigma is 1, assuming pixel measurements NonlinearFactorGraph graph; - for (size_t i = 0; i < data.number_tracks(); i++) + for (size_t i = 0; i < data.nrTracks(); i++) graph.emplace_shared(100, i, pA(i), pB(i), model2, K); // Check error at ground truth Values truth; truth.insert(100, trueE); - for (size_t i = 0; i < data.number_tracks(); i++) { + for (size_t i = 0; i < data.nrTracks(); i++) { Point3 P1 = data.tracks[i].p; truth.insert(i, double(baseline / P1.z())); } @@ -654,7 +654,7 @@ TEST(EssentialMatrixFactor2, extraMinimization) { // Check result EssentialMatrix actual = result.at(100); EXPECT(assert_equal(trueE, actual, 1e-1)); - for (size_t i = 0; i < data.number_tracks(); i++) + for (size_t i = 0; i < data.nrTracks(); i++) EXPECT_DOUBLES_EQUAL(truth.at(i), result.at(i), 1e-1); // Check error at result diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index f3e48c3c3a..1c4d76c837 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -29,10 +29,10 @@ def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None: """Plot the SFM results.""" plot_vals = gtsam.Values() - for cam_idx in range(scene_data.number_cameras()): + for cam_idx in range(scene_data.nrCameras()): plot_vals.insert(C(cam_idx), result.atPinholeCameraCal3Bundler(C(cam_idx)).pose()) - for j in range(scene_data.number_tracks()): + for j in range(scene_data.nrTracks()): plot_vals.insert(P(j), result.atPoint3(P(j))) plot.plot_3d_points(0, plot_vals, linespec="g.") @@ -47,8 +47,8 @@ def run(args: argparse.Namespace) -> None: # Load the SfM data from file scene_data = gtsam.readBal(input_file) - logging.info("read %d tracks on %d cameras\n", scene_data.number_tracks(), - scene_data.number_cameras()) + logging.info("read %d tracks on %d cameras\n", scene_data.nrTracks(), + scene_data.nrCameras()) # Create a factor graph graph = gtsam.NonlinearFactorGraph() @@ -57,10 +57,10 @@ def run(args: argparse.Namespace) -> None: noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v # Add measurements to the factor graph - for j in range(scene_data.number_tracks()): + for j in range(scene_data.nrTracks()): track = scene_data.track(j) # SfmTrack # retrieve the SfmMeasurement objects - for m_idx in range(track.number_measurements()): + for m_idx in range(track.nrMeasurements()): # 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 @@ -82,13 +82,13 @@ def run(args: argparse.Namespace) -> None: i = 0 # add each PinholeCameraCal3Bundler - for cam_idx in range(scene_data.number_cameras()): + for cam_idx in range(scene_data.nrCameras()): camera = scene_data.camera(cam_idx) initial.insert(C(i), camera) i += 1 # add each SfmTrack - for j in range(scene_data.number_tracks()): + for j in range(scene_data.nrTracks()): track = scene_data.track(j) initial.insert(P(j), track.point3()) diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py index 9c965ddc5d..79004f03c9 100644 --- a/python/gtsam/tests/test_SfmData.py +++ b/python/gtsam/tests/test_SfmData.py @@ -39,10 +39,10 @@ def test_tracks(self): # translating point uv_i1 along X-axis uv_i2 = gtsam.Point2(24.88, 82) # add measurements to the track - self.tracks.add_measurement(i1, uv_i1) - self.tracks.add_measurement(i2, uv_i2) + self.tracks.addMeasurement(i1, uv_i1) + self.tracks.addMeasurement(i2, uv_i2) # Number of measurements in the track is 2 - self.assertEqual(self.tracks.number_measurements(), 2) + self.assertEqual(self.tracks.nrMeasurements(), 2) # camera_idx in the first measurement of the track corresponds to i1 cam_idx, img_measurement = self.tracks.measurement(0) self.assertEqual(cam_idx, i1) @@ -64,13 +64,13 @@ def test_data(self): measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)] pt = gtsam.Point3(1.0, 6.0, 2.0) track2 = gtsam.SfmTrack(pt) - track2.add_measurement(i1, uv_i1) - track2.add_measurement(i2, uv_i2) - track2.add_measurement(i3, uv_i3) - self.data.add_track(self.tracks) - self.data.add_track(track2) + track2.addMeasurement(i1, uv_i1) + track2.addMeasurement(i2, uv_i2) + track2.addMeasurement(i3, uv_i3) + self.data.addTrack(self.tracks) + self.data.addTrack(track2) # Number of tracks in SfmData is 2 - self.assertEqual(self.data.number_tracks(), 2) + self.assertEqual(self.data.nrTracks(), 2) # camera idx of first measurement of second track corresponds to i1 cam_idx, img_measurement = self.data.track(1).measurement(0) self.assertEqual(cam_idx, i1) diff --git a/python/gtsam/tests/test_pickle.py b/python/gtsam/tests/test_pickle.py index 0acbf6765e..a6a5745bca 100644 --- a/python/gtsam/tests/test_pickle.py +++ b/python/gtsam/tests/test_pickle.py @@ -37,8 +37,8 @@ def test_pose3_roundtrip(self): def test_sfmTrack_roundtrip(self): obj = SfmTrack(Point3(1, 1, 0)) - obj.add_measurement(0, Point2(-1, 5)) - obj.add_measurement(1, Point2(6, 2)) + obj.addMeasurement(0, Point2(-1, 5)) + obj.addMeasurement(1, Point2(6, 2)) self.assertEqualityOnPickleRoundtrip(obj) def test_unit3_roundtrip(self): diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp index 05b4c7f661..895c8484ba 100644 --- a/tests/testGeneralSFMFactorB.cpp +++ b/tests/testGeneralSFMFactorB.cpp @@ -49,7 +49,7 @@ TEST(PinholeCamera, BAL) { SharedNoiseModel unit2 = noiseModel::Unit::Create(2); NonlinearFactorGraph graph; - for (size_t j = 0; j < db.number_tracks(); j++) { + for (size_t j = 0; j < db.nrTracks(); j++) { for (const SfmMeasurement& m: db.tracks[j].measurements) graph.emplace_shared(m.second, unit2, m.first, P(j)); } diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 4a58a57a66..06b9637572 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -36,7 +36,7 @@ int main(int argc, char* argv[]) { // Build graph using conventional GeneralSFMFactor NonlinearFactorGraph graph; - for (size_t j = 0; j < db.number_tracks(); j++) { + for (size_t j = 0; j < db.nrTracks(); j++) { for (const SfmMeasurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index 548c4de700..3108624ce5 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -73,8 +73,8 @@ int optimize(const SfmData& db, const NonlinearFactorGraph& graph, if (gUseSchur) { // 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++) { + for (size_t j = 0; j < db.nrTracks(); j++) ordering.push_back(P(j)); + for (size_t i = 0; i < db.nrCameras(); i++) { ordering.push_back(C(i)); if (separateCalibration) ordering.push_back(K(i)); } diff --git a/timing/timeSFMBALautodiff.cpp b/timing/timeSFMBALautodiff.cpp index 2d0f4a1fea..63e1931459 100644 --- a/timing/timeSFMBALautodiff.cpp +++ b/timing/timeSFMBALautodiff.cpp @@ -44,7 +44,7 @@ int main(int argc, char* argv[]) { // Build graph NonlinearFactorGraph graph; - for (size_t j = 0; j < db.number_tracks(); j++) { + for (size_t j = 0; j < db.nrTracks(); j++) { for (const SfmMeasurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; diff --git a/timing/timeSFMBALcamTnav.cpp b/timing/timeSFMBALcamTnav.cpp index 355defed9a..cc26710403 100644 --- a/timing/timeSFMBALcamTnav.cpp +++ b/timing/timeSFMBALcamTnav.cpp @@ -33,7 +33,7 @@ int main(int argc, char* argv[]) { // Build graph using conventional GeneralSFMFactor NonlinearFactorGraph graph; - for (size_t j = 0; j < db.number_tracks(); j++) { + for (size_t j = 0; j < db.nrTracks(); j++) { for (const SfmMeasurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; diff --git a/timing/timeSFMBALnavTcam.cpp b/timing/timeSFMBALnavTcam.cpp index e602ef2410..4c3bc06214 100644 --- a/timing/timeSFMBALnavTcam.cpp +++ b/timing/timeSFMBALnavTcam.cpp @@ -33,7 +33,7 @@ int main(int argc, char* argv[]) { // Build graph using conventional GeneralSFMFactor NonlinearFactorGraph graph; - for (size_t j = 0; j < db.number_tracks(); j++) { + for (size_t j = 0; j < db.nrTracks(); j++) { Point3_ nav_point_(P(j)); for (const SfmMeasurement& m: db.tracks[j].measurements) { size_t i = m.first; diff --git a/timing/timeSFMBALsmart.cpp b/timing/timeSFMBALsmart.cpp index a69d895a5d..f37f35a394 100644 --- a/timing/timeSFMBALsmart.cpp +++ b/timing/timeSFMBALsmart.cpp @@ -35,7 +35,7 @@ int main(int argc, char* argv[]) { // Add smart factors to graph NonlinearFactorGraph graph; - for (size_t j = 0; j < db.number_tracks(); j++) { + for (size_t j = 0; j < db.nrTracks(); j++) { auto smartFactor = boost::make_shared(gNoiseModel); for (const SfmMeasurement& m : db.tracks[j].measurements) { size_t i = m.first; From 10fa7387a7132f5ef47daf64cd9d4d2874b7d1fd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Jan 2022 08:36:56 -0500 Subject: [PATCH 04/10] Fix test --- python/gtsam/tests/test_GraphvizFormatting.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python/gtsam/tests/test_GraphvizFormatting.py b/python/gtsam/tests/test_GraphvizFormatting.py index 5962366efa..ecdc23b450 100644 --- a/python/gtsam/tests/test_GraphvizFormatting.py +++ b/python/gtsam/tests/test_GraphvizFormatting.py @@ -78,7 +78,7 @@ def test_swapped_axes(self): graphviz_formatting.paperHorizontalAxis = gtsam.GraphvizFormatting.Axis.X graphviz_formatting.paperVerticalAxis = gtsam.GraphvizFormatting.Axis.Y self.assertEqual(self.graph.dot(self.values, - formatting=graphviz_formatting), + writer=graphviz_formatting), textwrap.dedent(expected_result)) def test_factor_points(self): @@ -100,7 +100,7 @@ def test_factor_points(self): graphviz_formatting.plotFactorPoints = False self.assertEqual(self.graph.dot(self.values, - formatting=graphviz_formatting), + writer=graphviz_formatting), textwrap.dedent(expected_result)) def test_width_height(self): @@ -127,7 +127,7 @@ def test_width_height(self): graphviz_formatting.figureHeightInches = 10 self.assertEqual(self.graph.dot(self.values, - formatting=graphviz_formatting), + writer=graphviz_formatting), textwrap.dedent(expected_result)) From 3090182132f93244b359e3796f5b1a6cc62ff008 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Jan 2022 09:21:13 -0500 Subject: [PATCH 05/10] Moved loading code --- gtsam/sfm/SfmData.cpp | 361 ++++++++++++++++++++++++++++++++++++++++- gtsam/sfm/SfmData.h | 92 +++++++++++ gtsam/slam/dataset.cpp | 348 --------------------------------------- gtsam/slam/dataset.h | 90 ---------- 4 files changed, 451 insertions(+), 440 deletions(-) diff --git a/gtsam/sfm/SfmData.cpp b/gtsam/sfm/SfmData.cpp index 715dda2ed4..1e3d53601e 100644 --- a/gtsam/sfm/SfmData.cpp +++ b/gtsam/sfm/SfmData.cpp @@ -16,16 +16,28 @@ * @brief Data structure for dealing with Structure from Motion data */ +#include #include +#include +#include + namespace gtsam { -void SfmData::print(const std::string& s) const { +using std::cout; +using std::endl; + +using gtsam::symbol_shorthand::P; +using gtsam::symbol_shorthand::X; + +/* ************************************************************************** */ +void SfmData::print(const std::string &s) const { std::cout << "Number of cameras = " << nrCameras() << std::endl; std::cout << "Number of tracks = " << nrTracks() << std::endl; } -bool SfmData::equals(const SfmData& sfmData, double tol) const { +/* ************************************************************************** */ +bool SfmData::equals(const SfmData &sfmData, double tol) const { // check number of cameras and tracks if (nrCameras() != sfmData.nrCameras() || nrTracks() != sfmData.nrTracks()) { return false; @@ -48,4 +60,349 @@ bool SfmData::equals(const SfmData& sfmData, double tol) const { return true; } +/* ************************************************************************* */ +Rot3 openGLFixedRotation() { // this is due to different convention for + // cameras in gtsam and openGL + /* R = [ 1 0 0 + * 0 -1 0 + * 0 0 -1] + */ + Matrix3 R_mat = Matrix3::Zero(3, 3); + R_mat(0, 0) = 1.0; + R_mat(1, 1) = -1.0; + R_mat(2, 2) = -1.0; + return Rot3(R_mat); +} + +/* ************************************************************************* */ +Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz) { + Rot3 R90 = openGLFixedRotation(); + Rot3 wRc = (R.inverse()).compose(R90); + + // Our camera-to-world translation wTc = -R'*t + return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz))); +} + +/* ************************************************************************* */ +Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz) { + Rot3 R90 = openGLFixedRotation(); + Rot3 cRw_openGL = R90.compose(R.inverse()); + Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz)); + return Pose3(cRw_openGL, t_openGL); +} + +/* ************************************************************************* */ +Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) { + return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), + PoseGTSAM.z()); +} + +/* ************************************************************************** */ +bool readBundler(const std::string &filename, SfmData &data) { + // Load the data file + std::ifstream is(filename.c_str(), std::ifstream::in); + if (!is) { + cout << "Error in readBundler: can not find the file!!" << endl; + return false; + } + + // Ignore the first line + char aux[500]; + is.getline(aux, 500); + + // Get the number of camera poses and 3D points + size_t nrPoses, nrPoints; + is >> nrPoses >> nrPoints; + + // Get the information for the camera poses + for (size_t i = 0; i < nrPoses; i++) { + // Get the focal length and the radial distortion parameters + float f, k1, k2; + is >> f >> k1 >> k2; + Cal3Bundler K(f, k1, k2); + + // Get the rotation matrix + float r11, r12, r13; + float r21, r22, r23; + float r31, r32, r33; + is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33; + + // Bundler-OpenGL rotation matrix + Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33); + + // Check for all-zero R, in which case quit + if (r11 == 0 && r12 == 0 && r13 == 0) { + cout << "Error in readBundler: zero rotation matrix for pose " << i + << endl; + return false; + } + + // Get the translation vector + float tx, ty, tz; + is >> tx >> ty >> tz; + + Pose3 pose = openGL2gtsam(R, tx, ty, tz); + + data.cameras.emplace_back(pose, K); + } + + // Get the information for the 3D points + data.tracks.reserve(nrPoints); + for (size_t j = 0; j < nrPoints; j++) { + SfmTrack track; + + // Get the 3D position + float x, y, z; + is >> x >> y >> z; + track.p = Point3(x, y, z); + + // Get the color information + float r, g, b; + is >> r >> g >> b; + track.r = r / 255.f; + track.g = g / 255.f; + track.b = b / 255.f; + + // Now get the visibility information + size_t nvisible = 0; + is >> nvisible; + + track.measurements.reserve(nvisible); + track.siftIndices.reserve(nvisible); + for (size_t k = 0; k < nvisible; k++) { + size_t cam_idx = 0, point_idx = 0; + float u, v; + is >> cam_idx >> point_idx >> u >> v; + track.measurements.emplace_back(cam_idx, Point2(u, -v)); + track.siftIndices.emplace_back(cam_idx, point_idx); + } + + data.tracks.push_back(track); + } + + is.close(); + return true; +} + +/* ************************************************************************** */ +bool readBAL(const std::string &filename, SfmData &data) { + // Load the data file + std::ifstream is(filename.c_str(), std::ifstream::in); + if (!is) { + cout << "Error in readBAL: can not find the file!!" << endl; + return false; + } + + // Get the number of camera poses and 3D points + size_t nrPoses, nrPoints, nrObservations; + is >> nrPoses >> nrPoints >> nrObservations; + + data.tracks.resize(nrPoints); + + // Get the information for the observations + for (size_t k = 0; k < nrObservations; k++) { + size_t i = 0, j = 0; + float u, v; + is >> i >> j >> u >> v; + data.tracks[j].measurements.emplace_back(i, Point2(u, -v)); + } + + // Get the information for the camera poses + for (size_t i = 0; i < nrPoses; i++) { + // Get the Rodrigues vector + float wx, wy, wz; + is >> wx >> wy >> wz; + Rot3 R = Rot3::Rodrigues(wx, wy, wz); // BAL-OpenGL rotation matrix + + // Get the translation vector + float tx, ty, tz; + is >> tx >> ty >> tz; + + Pose3 pose = openGL2gtsam(R, tx, ty, tz); + + // Get the focal length and the radial distortion parameters + float f, k1, k2; + is >> f >> k1 >> k2; + Cal3Bundler K(f, k1, k2); + + data.cameras.emplace_back(pose, K); + } + + // Get the information for the 3D points + for (size_t j = 0; j < nrPoints; j++) { + // Get the 3D position + float x, y, z; + is >> x >> y >> z; + SfmTrack &track = data.tracks[j]; + track.p = Point3(x, y, z); + track.r = 0.4f; + track.g = 0.4f; + track.b = 0.4f; + } + + is.close(); + return true; +} + +/* ************************************************************************** */ +SfmData readBal(const std::string &filename) { + SfmData data; + readBAL(filename, data); + return data; +} + +/* ************************************************************************** */ +bool writeBAL(const std::string &filename, SfmData &data) { + // Open the output file + std::ofstream os; + os.open(filename.c_str()); + os.precision(20); + if (!os.is_open()) { + cout << "Error in writeBAL: can not open the file!!" << endl; + return false; + } + + // Write the number of camera poses and 3D points + size_t nrObservations = 0; + for (size_t j = 0; j < data.nrTracks(); j++) { + nrObservations += data.tracks[j].nrMeasurements(); + } + + // Write observations + os << data.nrCameras() << " " << data.nrTracks() << " " << nrObservations + << endl; + os << endl; + + for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j + const SfmTrack &track = data.tracks[j]; + + for (size_t k = 0; k < track.nrMeasurements(); + k++) { // for each observation of the 3D point j + size_t i = track.measurements[k].first; // camera id + double u0 = data.cameras[i].calibration().px(); + double v0 = data.cameras[i].calibration().py(); + + if (u0 != 0 || v0 != 0) { + cout << "writeBAL has not been tested for calibration with nonzero " + "(u0,v0)" + << endl; + } + + double pixelBALx = track.measurements[k].second.x() - + u0; // center of image is the origin + double pixelBALy = -(track.measurements[k].second.y() - + v0); // center of image is the origin + Point2 pixelMeasurement(pixelBALx, pixelBALy); + os << i /*camera id*/ << " " << j /*point id*/ << " " + << pixelMeasurement.x() /*u of the pixel*/ << " " + << pixelMeasurement.y() /*v of the pixel*/ << endl; + } + } + os << endl; + + // Write cameras + for (size_t i = 0; i < data.nrCameras(); i++) { // for each camera + Pose3 poseGTSAM = data.cameras[i].pose(); + Cal3Bundler cameraCalibration = data.cameras[i].calibration(); + Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); + os << Rot3::Logmap(poseOpenGL.rotation()) << endl; + os << poseOpenGL.translation().x() << endl; + os << poseOpenGL.translation().y() << endl; + os << poseOpenGL.translation().z() << endl; + os << cameraCalibration.fx() << endl; + os << cameraCalibration.k1() << endl; + os << cameraCalibration.k2() << endl; + os << endl; + } + + // Write the points + for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j + Point3 point = data.tracks[j].p; + os << point.x() << endl; + os << point.y() << endl; + os << point.z() << endl; + os << endl; + } + + os.close(); + return true; +} + +/* ************************************************************************** */ +bool writeBALfromValues(const std::string &filename, const SfmData &data, + Values &values) { + using Camera = PinholeCamera; + SfmData dataValues = data; + + // Store poses or cameras in SfmData + size_t nrPoses = values.count(); + if (nrPoses == dataValues.nrCameras()) { // we only estimated camera poses + for (size_t i = 0; i < dataValues.nrCameras(); i++) { // for each camera + Pose3 pose = values.at(X(i)); + Cal3Bundler K = dataValues.cameras[i].calibration(); + Camera camera(pose, K); + dataValues.cameras[i] = camera; + } + } else { + size_t nrCameras = values.count(); + if (nrCameras == dataValues.nrCameras()) { // we only estimated camera + // poses and calibration + for (size_t i = 0; i < nrCameras; i++) { // for each camera + Key cameraKey = i; // symbol('c',i); + Camera camera = values.at(cameraKey); + dataValues.cameras[i] = camera; + } + } else { + cout << "writeBALfromValues: different number of cameras in " + "SfM_dataValues (#cameras " + << dataValues.nrCameras() << ") and values (#cameras " << nrPoses + << ", #poses " << nrCameras << ")!!" << endl; + return false; + } + } + + // Store 3D points in SfmData + size_t nrPoints = values.count(), nrTracks = dataValues.nrTracks(); + if (nrPoints != nrTracks) { + cout << "writeBALfromValues: different number of points in " + "SfM_dataValues (#points= " + << nrTracks << ") and values (#points " << nrPoints << ")!!" << endl; + } + + for (size_t j = 0; j < nrTracks; j++) { // for each point + Key pointKey = P(j); + if (values.exists(pointKey)) { + Point3 point = values.at(pointKey); + dataValues.tracks[j].p = point; + } else { + dataValues.tracks[j].r = 1.0; + dataValues.tracks[j].g = 0.0; + dataValues.tracks[j].b = 0.0; + dataValues.tracks[j].p = Point3(0, 0, 0); + } + } + + // Write SfmData to file + return writeBAL(filename, dataValues); +} + +/* ************************************************************************** */ +Values initialCamerasEstimate(const SfmData &db) { + Values initial; + size_t i = 0; // NO POINTS: j = 0; + for (const SfmCamera &camera : db.cameras) initial.insert(i++, camera); + return initial; +} + +/* ************************************************************************** */ +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 SfmTrack &track : db.tracks) initial.insert(P(j++), track.p); + return initial; +} + +/* ************************************************************************** */ + } // namespace gtsam diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h index 91b7a5d1ee..8770fcd4a2 100644 --- a/gtsam/sfm/SfmData.h +++ b/gtsam/sfm/SfmData.h @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -100,4 +101,95 @@ struct SfmData { template <> struct traits : public Testable {}; +/** + * @brief This function parses a bundler output file and stores the data into a + * SfmData structure + * @param filename The name of the bundler file + * @param data SfM structure where the data is stored + * @return true if the parsing was successful, false otherwise + */ +GTSAM_EXPORT bool readBundler(const std::string& filename, SfmData& data); + +/** + * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and + * stores the data into a SfmData structure + * @param filename The name of the BAL file + * @param data SfM structure where the data is stored + * @return true if the parsing was successful, false otherwise + */ +GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData& data); + +/** + * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and + * returns the data as a SfmData structure. Mainly used by wrapped code. + * @param filename The name of the BAL file. + * @return SfM structure where the data is stored. + */ +GTSAM_EXPORT SfmData readBal(const std::string& filename); + +/** + * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file + * from a SfmData structure + * @param filename The name of the BAL file to write + * @param data SfM structure where the data is stored + * @return true if the parsing was successful, false otherwise + */ +GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData& data); + +/** + * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file + * from a SfmData structure and a value structure (measurements are the same as + * the SfM input data, while camera poses and values are read from Values) + * @param filename The name of the BAL file to write + * @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 + * @return true if the parsing was successful, false otherwise + */ +GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, + const SfmData& data, Values& values); + +/** + * @brief This function converts an openGL camera pose to an GTSAM camera pose + * @param R rotation in openGL + * @param tx x component of the translation in openGL + * @param ty y component of the translation in openGL + * @param tz z component of the translation in openGL + * @return Pose3 in GTSAM format + */ +GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz); + +/** + * @brief This function converts a GTSAM camera pose to an openGL camera pose + * @param R rotation in GTSAM + * @param tx x component of the translation in GTSAM + * @param ty y component of the translation in GTSAM + * @param tz z component of the translation in GTSAM + * @return Pose3 in openGL format + */ +GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz); + +/** + * @brief This function converts a GTSAM camera pose to an openGL camera pose + * @param PoseGTSAM pose in GTSAM format + * @return Pose3 in openGL format + */ +GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM); + +/** + * @brief This function creates initial values for cameras from db + * @param SfmData + * @return Values + */ +GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db); + +/** + * @brief This function creates initial values for cameras and points from db + * @param SfmData + * @return Values + */ +GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db); + } // namespace gtsam diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 8acdbfc14a..a2b96efab8 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -54,8 +54,6 @@ using namespace std; namespace fs = boost::filesystem; using gtsam::symbol_shorthand::L; -using gtsam::symbol_shorthand::P; -using gtsam::symbol_shorthand::X; #define LINESIZE 81920 @@ -945,352 +943,6 @@ GraphAndValues load3D(const string &filename) { return make_pair(graph, initial); } -/* ************************************************************************* */ -Rot3 openGLFixedRotation() { // this is due to different convention for - // cameras in gtsam and openGL - /* R = [ 1 0 0 - * 0 -1 0 - * 0 0 -1] - */ - Matrix3 R_mat = Matrix3::Zero(3, 3); - R_mat(0, 0) = 1.0; - R_mat(1, 1) = -1.0; - R_mat(2, 2) = -1.0; - return Rot3(R_mat); -} - -/* ************************************************************************* */ -Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz) { - Rot3 R90 = openGLFixedRotation(); - Rot3 wRc = (R.inverse()).compose(R90); - - // Our camera-to-world translation wTc = -R'*t - return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz))); -} - -/* ************************************************************************* */ -Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz) { - Rot3 R90 = openGLFixedRotation(); - Rot3 cRw_openGL = R90.compose(R.inverse()); - Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz)); - return Pose3(cRw_openGL, t_openGL); -} - -/* ************************************************************************* */ -Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) { - return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), - PoseGTSAM.z()); -} - -/* ************************************************************************* */ -bool readBundler(const string &filename, SfmData &data) { - // Load the data file - ifstream is(filename.c_str(), ifstream::in); - if (!is) { - cout << "Error in readBundler: can not find the file!!" << endl; - return false; - } - - // Ignore the first line - char aux[500]; - is.getline(aux, 500); - - // Get the number of camera poses and 3D points - size_t nrPoses, nrPoints; - is >> nrPoses >> nrPoints; - - // Get the information for the camera poses - for (size_t i = 0; i < nrPoses; i++) { - // Get the focal length and the radial distortion parameters - float f, k1, k2; - is >> f >> k1 >> k2; - Cal3Bundler K(f, k1, k2); - - // Get the rotation matrix - float r11, r12, r13; - float r21, r22, r23; - float r31, r32, r33; - is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33; - - // Bundler-OpenGL rotation matrix - Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33); - - // Check for all-zero R, in which case quit - if (r11 == 0 && r12 == 0 && r13 == 0) { - cout << "Error in readBundler: zero rotation matrix for pose " << i - << endl; - return false; - } - - // Get the translation vector - float tx, ty, tz; - is >> tx >> ty >> tz; - - Pose3 pose = openGL2gtsam(R, tx, ty, tz); - - data.cameras.emplace_back(pose, K); - } - - // Get the information for the 3D points - data.tracks.reserve(nrPoints); - for (size_t j = 0; j < nrPoints; j++) { - SfmTrack track; - - // Get the 3D position - float x, y, z; - is >> x >> y >> z; - track.p = Point3(x, y, z); - - // Get the color information - float r, g, b; - is >> r >> g >> b; - track.r = r / 255.f; - track.g = g / 255.f; - track.b = b / 255.f; - - // Now get the visibility information - size_t nvisible = 0; - is >> nvisible; - - track.measurements.reserve(nvisible); - track.siftIndices.reserve(nvisible); - for (size_t k = 0; k < nvisible; k++) { - size_t cam_idx = 0, point_idx = 0; - float u, v; - is >> cam_idx >> point_idx >> u >> v; - track.measurements.emplace_back(cam_idx, Point2(u, -v)); - track.siftIndices.emplace_back(cam_idx, point_idx); - } - - data.tracks.push_back(track); - } - - is.close(); - return true; -} - -/* ************************************************************************* */ -bool readBAL(const string &filename, SfmData &data) { - // Load the data file - ifstream is(filename.c_str(), ifstream::in); - if (!is) { - cout << "Error in readBAL: can not find the file!!" << endl; - return false; - } - - // Get the number of camera poses and 3D points - size_t nrPoses, nrPoints, nrObservations; - is >> nrPoses >> nrPoints >> nrObservations; - - data.tracks.resize(nrPoints); - - // Get the information for the observations - for (size_t k = 0; k < nrObservations; k++) { - size_t i = 0, j = 0; - float u, v; - is >> i >> j >> u >> v; - data.tracks[j].measurements.emplace_back(i, Point2(u, -v)); - } - - // Get the information for the camera poses - for (size_t i = 0; i < nrPoses; i++) { - // Get the Rodrigues vector - float wx, wy, wz; - is >> wx >> wy >> wz; - Rot3 R = Rot3::Rodrigues(wx, wy, wz); // BAL-OpenGL rotation matrix - - // Get the translation vector - float tx, ty, tz; - is >> tx >> ty >> tz; - - Pose3 pose = openGL2gtsam(R, tx, ty, tz); - - // Get the focal length and the radial distortion parameters - float f, k1, k2; - is >> f >> k1 >> k2; - Cal3Bundler K(f, k1, k2); - - data.cameras.emplace_back(pose, K); - } - - // Get the information for the 3D points - for (size_t j = 0; j < nrPoints; j++) { - // Get the 3D position - float x, y, z; - is >> x >> y >> z; - SfmTrack &track = data.tracks[j]; - track.p = Point3(x, y, z); - track.r = 0.4f; - track.g = 0.4f; - track.b = 0.4f; - } - - is.close(); - return true; -} - -/* ************************************************************************* */ -SfmData readBal(const string &filename) { - SfmData data; - readBAL(filename, data); - return data; -} - -/* ************************************************************************* */ -bool writeBAL(const string &filename, SfmData &data) { - // Open the output file - ofstream os; - os.open(filename.c_str()); - os.precision(20); - if (!os.is_open()) { - cout << "Error in writeBAL: can not open the file!!" << endl; - return false; - } - - // Write the number of camera poses and 3D points - size_t nrObservations = 0; - for (size_t j = 0; j < data.nrTracks(); j++) { - nrObservations += data.tracks[j].nrMeasurements(); - } - - // Write observations - os << data.nrCameras() << " " << data.nrTracks() << " " - << nrObservations << endl; - os << endl; - - for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j - const SfmTrack &track = data.tracks[j]; - - for (size_t k = 0; k < track.nrMeasurements(); - k++) { // for each observation of the 3D point j - size_t i = track.measurements[k].first; // camera id - double u0 = data.cameras[i].calibration().px(); - double v0 = data.cameras[i].calibration().py(); - - if (u0 != 0 || v0 != 0) { - cout << "writeBAL has not been tested for calibration with nonzero " - "(u0,v0)" - << endl; - } - - double pixelBALx = track.measurements[k].second.x() - - u0; // center of image is the origin - double pixelBALy = -(track.measurements[k].second.y() - - v0); // center of image is the origin - Point2 pixelMeasurement(pixelBALx, pixelBALy); - os << i /*camera id*/ << " " << j /*point id*/ << " " - << pixelMeasurement.x() /*u of the pixel*/ << " " - << pixelMeasurement.y() /*v of the pixel*/ << endl; - } - } - os << endl; - - // Write cameras - for (size_t i = 0; i < data.nrCameras(); i++) { // for each camera - Pose3 poseGTSAM = data.cameras[i].pose(); - Cal3Bundler cameraCalibration = data.cameras[i].calibration(); - Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); - os << Rot3::Logmap(poseOpenGL.rotation()) << endl; - os << poseOpenGL.translation().x() << endl; - os << poseOpenGL.translation().y() << endl; - os << poseOpenGL.translation().z() << endl; - os << cameraCalibration.fx() << endl; - os << cameraCalibration.k1() << endl; - os << cameraCalibration.k2() << endl; - os << endl; - } - - // Write the points - for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j - Point3 point = data.tracks[j].p; - os << point.x() << endl; - os << point.y() << endl; - os << point.z() << endl; - os << endl; - } - - os.close(); - return true; -} - -bool writeBALfromValues(const string &filename, const SfmData &data, - Values &values) { - using Camera = PinholeCamera; - SfmData dataValues = data; - - // Store poses or cameras in SfmData - size_t nrPoses = values.count(); - if (nrPoses == - dataValues.nrCameras()) { // we only estimated camera poses - for (size_t i = 0; i < dataValues.nrCameras(); - i++) { // for each camera - Pose3 pose = values.at(X(i)); - Cal3Bundler K = dataValues.cameras[i].calibration(); - Camera camera(pose, K); - dataValues.cameras[i] = camera; - } - } else { - size_t nrCameras = values.count(); - if (nrCameras == dataValues.nrCameras()) { // we only estimated camera - // poses and calibration - for (size_t i = 0; i < nrCameras; i++) { // for each camera - Key cameraKey = i; // symbol('c',i); - Camera camera = values.at(cameraKey); - dataValues.cameras[i] = camera; - } - } else { - cout << "writeBALfromValues: different number of cameras in " - "SfM_dataValues (#cameras " - << dataValues.nrCameras() << ") and values (#cameras " - << nrPoses << ", #poses " << nrCameras << ")!!" << endl; - return false; - } - } - - // Store 3D points in SfmData - size_t nrPoints = values.count(), - nrTracks = dataValues.nrTracks(); - if (nrPoints != nrTracks) { - cout << "writeBALfromValues: different number of points in " - "SfM_dataValues (#points= " - << nrTracks << ") and values (#points " << nrPoints << ")!!" << endl; - } - - for (size_t j = 0; j < nrTracks; j++) { // for each point - Key pointKey = P(j); - if (values.exists(pointKey)) { - Point3 point = values.at(pointKey); - dataValues.tracks[j].p = point; - } else { - dataValues.tracks[j].r = 1.0; - dataValues.tracks[j].g = 0.0; - dataValues.tracks[j].b = 0.0; - dataValues.tracks[j].p = Point3(0, 0, 0); - } - } - - // Write SfmData to file - return writeBAL(filename, dataValues); -} - -Values initialCamerasEstimate(const SfmData &db) { - Values initial; - size_t i = 0; // NO POINTS: j = 0; - for (const SfmCamera &camera : db.cameras) - initial.insert(i++, camera); - return initial; -} - -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 SfmTrack &track : db.tracks) - initial.insert(P(j++), track.p); - return initial; -} - // Wrapper-friendly versions of parseFactors and parseFactors BetweenFactorPose2s parse2DFactors(const std::string &filename, diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index d5c93c7f83..dc450a9f77 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -209,96 +209,6 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, /// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); -/** - * @brief This function parses a bundler output file and stores the data into a - * SfmData structure - * @param filename The name of the bundler file - * @param data SfM structure where the data is stored - * @return true if the parsing was successful, false otherwise - */ -GTSAM_EXPORT bool readBundler(const std::string& filename, SfmData &data); - -/** - * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a - * SfmData structure - * @param filename The name of the BAL file - * @param data SfM structure where the data is stored - * @return true if the parsing was successful, false otherwise - */ -GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData &data); - -/** - * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data - * as a SfmData structure. Mainly used by wrapped code. - * @param filename The name of the BAL file. - * @return SfM structure where the data is stored. - */ -GTSAM_EXPORT SfmData readBal(const std::string& filename); - -/** - * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a - * SfmData structure - * @param filename The name of the BAL file to write - * @param data SfM structure where the data is stored - * @return true if the parsing was successful, false otherwise - */ -GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData &data); - -/** - * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a - * SfmData structure and a value structure (measurements are the same as the SfM input data, - * while camera poses and values are read from Values) - * @param filename The name of the BAL file to write - * @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 - * @return true if the parsing was successful, false otherwise - */ -GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, - const SfmData &data, Values& values); - -/** - * @brief This function converts an openGL camera pose to an GTSAM camera pose - * @param R rotation in openGL - * @param tx x component of the translation in openGL - * @param ty y component of the translation in openGL - * @param tz z component of the translation in openGL - * @return Pose3 in GTSAM format - */ -GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz); - -/** - * @brief This function converts a GTSAM camera pose to an openGL camera pose - * @param R rotation in GTSAM - * @param tx x component of the translation in GTSAM - * @param ty y component of the translation in GTSAM - * @param tz z component of the translation in GTSAM - * @return Pose3 in openGL format - */ -GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz); - -/** - * @brief This function converts a GTSAM camera pose to an openGL camera pose - * @param PoseGTSAM pose in GTSAM format - * @return Pose3 in openGL format - */ -GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM); - -/** - * @brief This function creates initial values for cameras from db - * @param SfmData - * @return Values - */ -GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db); - -/** - * @brief This function creates initial values for cameras and points from db - * @param SfmData - * @return Values - */ -GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db); - // Wrapper-friendly versions of parseFactors and parseFactors using BetweenFactorPose2s = std::vector::shared_ptr>; GTSAM_EXPORT BetweenFactorPose2s From 6fbb948a7de6d42aec08dd39bd4c019f2a400338 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Jan 2022 09:30:00 -0500 Subject: [PATCH 06/10] moved tests --- gtsam/sfm/tests/testSfmData.cpp | 211 +++++++++++++++++++++++++++++++ gtsam/slam/tests/testDataset.cpp | 175 ------------------------- 2 files changed, 211 insertions(+), 175 deletions(-) create mode 100644 gtsam/sfm/tests/testSfmData.cpp diff --git a/gtsam/sfm/tests/testSfmData.cpp b/gtsam/sfm/tests/testSfmData.cpp new file mode 100644 index 0000000000..f19542c260 --- /dev/null +++ b/gtsam/sfm/tests/testSfmData.cpp @@ -0,0 +1,211 @@ +/* ---------------------------------------------------------------------------- + + * 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 TestSfmData.cpp + * @date January 2022 + * @author Frank dellaert + * @brief tests for SfmData class and associated utilites + */ + +#include +#include +#include + +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); +GTSAM_EXPORT std::string findExampleDataFile(const std::string& name); +} // namespace gtsam + +/* ************************************************************************* */ +TEST(dataSet, Balbianello) { + ///< The structure where we will save the SfM data + const string filename = findExampleDataFile("Balbianello"); + SfmData mydata; + CHECK(readBundler(filename, mydata)); + + // Check number of things + EXPECT_LONGS_EQUAL(5, mydata.nrCameras()); + EXPECT_LONGS_EQUAL(544, mydata.nrTracks()); + const SfmTrack& track0 = mydata.tracks[0]; + EXPECT_LONGS_EQUAL(3, track0.nrMeasurements()); + + // Check projection of a given point + EXPECT_LONGS_EQUAL(0, track0.measurements[0].first); + const SfmCamera& camera0 = mydata.cameras[0]; + Point2 expected = camera0.project(track0.p), + actual = track0.measurements[0].second; + EXPECT(assert_equal(expected, actual, 1)); +} + +/* ************************************************************************* */ +TEST(dataSet, readBAL_Dubrovnik) { + ///< The structure where we will save the SfM data + const string filename = findExampleDataFile("dubrovnik-3-7-pre"); + SfmData mydata; + CHECK(readBAL(filename, mydata)); + + // Check number of things + EXPECT_LONGS_EQUAL(3, mydata.nrCameras()); + EXPECT_LONGS_EQUAL(7, mydata.nrTracks()); + const SfmTrack& track0 = mydata.tracks[0]; + EXPECT_LONGS_EQUAL(3, track0.nrMeasurements()); + + // Check projection of a given point + EXPECT_LONGS_EQUAL(0, track0.measurements[0].first); + const SfmCamera& camera0 = mydata.cameras[0]; + Point2 expected = camera0.project(track0.p), + actual = track0.measurements[0].second; + EXPECT(assert_equal(expected, actual, 12)); +} + +/* ************************************************************************* */ +TEST(dataSet, openGL2gtsam) { + Vector3 rotVec(0.2, 0.7, 1.1); + Rot3 R = Rot3::Expmap(rotVec); + Point3 t = Point3(0.0, 0.0, 0.0); + Pose3 poseGTSAM = Pose3(R, t); + + Pose3 expected = openGL2gtsam(R, t.x(), t.y(), t.z()); + + Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); // columns! + Rot3 cRw(r1.x(), r2.x(), r3.x(), -r1.y(), -r2.y(), -r3.y(), -r1.z(), -r2.z(), + -r3.z()); + Rot3 wRc = cRw.inverse(); + Pose3 actual = Pose3(wRc, t); + + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(dataSet, gtsam2openGL) { + Vector3 rotVec(0.2, 0.7, 1.1); + Rot3 R = Rot3::Expmap(rotVec); + Point3 t = Point3(1.0, 20.0, 10.0); + Pose3 actual = Pose3(R, t); + Pose3 poseGTSAM = openGL2gtsam(R, t.x(), t.y(), t.z()); + + Pose3 expected = gtsam2openGL(poseGTSAM); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(dataSet, writeBAL_Dubrovnik) { + ///< Read a file using the unit tested readBAL + const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); + SfmData readData; + readBAL(filenameToRead, readData); + + // Write readData to file filenameToWrite + const string filenameToWrite = createRewrittenFileName(filenameToRead); + CHECK(writeBAL(filenameToWrite, readData)); + + // Read what we wrote + SfmData writtenData; + CHECK(readBAL(filenameToWrite, writtenData)); + + // Check that what we read is the same as what we wrote + EXPECT_LONGS_EQUAL(readData.nrCameras(), writtenData.nrCameras()); + EXPECT_LONGS_EQUAL(readData.nrTracks(), writtenData.nrTracks()); + + for (size_t i = 0; i < readData.nrCameras(); i++) { + PinholeCamera expectedCamera = writtenData.cameras[i]; + PinholeCamera actualCamera = readData.cameras[i]; + EXPECT(assert_equal(expectedCamera, actualCamera)); + } + + for (size_t j = 0; j < readData.nrTracks(); j++) { + // check point + SfmTrack expectedTrack = writtenData.tracks[j]; + SfmTrack actualTrack = readData.tracks[j]; + Point3 expectedPoint = expectedTrack.p; + Point3 actualPoint = actualTrack.p; + EXPECT(assert_equal(expectedPoint, actualPoint)); + + // check rgb + Point3 expectedRGB = + Point3(expectedTrack.r, expectedTrack.g, expectedTrack.b); + Point3 actualRGB = Point3(actualTrack.r, actualTrack.g, actualTrack.b); + EXPECT(assert_equal(expectedRGB, actualRGB)); + + // check measurements + for (size_t k = 0; k < actualTrack.nrMeasurements(); k++) { + EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first, + actualTrack.measurements[k].first); + EXPECT(assert_equal(expectedTrack.measurements[k].second, + actualTrack.measurements[k].second)); + } + } +} + +/* ************************************************************************* */ +TEST(dataSet, writeBALfromValues_Dubrovnik) { + ///< Read a file using the unit tested readBAL + const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); + SfmData readData; + readBAL(filenameToRead, readData); + + Pose3 poseChange = + Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.3, 0.1, 0.3)); + + Values value; + for (size_t i = 0; i < readData.nrCameras(); i++) { // for each camera + Pose3 pose = poseChange.compose(readData.cameras[i].pose()); + value.insert(X(i), pose); + } + for (size_t j = 0; j < readData.nrTracks(); j++) { // for each point + Point3 point = poseChange.transformFrom(readData.tracks[j].p); + value.insert(P(j), point); + } + + // Write values and readData to a file + const string filenameToWrite = createRewrittenFileName(filenameToRead); + writeBALfromValues(filenameToWrite, readData, value); + + // Read the file we wrote + SfmData writtenData; + readBAL(filenameToWrite, writtenData); + + // Check that the reprojection errors are the same and the poses are correct + // Check number of things + EXPECT_LONGS_EQUAL(3, writtenData.nrCameras()); + EXPECT_LONGS_EQUAL(7, writtenData.nrTracks()); + const SfmTrack& track0 = writtenData.tracks[0]; + EXPECT_LONGS_EQUAL(3, track0.nrMeasurements()); + + // Check projection of a given point + EXPECT_LONGS_EQUAL(0, track0.measurements[0].first); + const SfmCamera& camera0 = writtenData.cameras[0]; + Point2 expected = camera0.project(track0.p), + actual = track0.measurements[0].second; + EXPECT(assert_equal(expected, actual, 12)); + + Pose3 expectedPose = camera0.pose(); + Pose3 actualPose = value.at(X(0)); + EXPECT(assert_equal(expectedPose, actualPose, 1e-7)); + + Point3 expectedPoint = track0.p; + Point3 actualPoint = value.at(P(0)); + EXPECT(assert_equal(expectedPoint, actualPoint, 1e-6)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index b88832ff2c..be638d51ad 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -151,27 +151,6 @@ TEST(dataSet, load2DVictoriaPark) { EXPECT_LONGS_EQUAL(L(5), graph->at(4)->keys()[1]); } -/* ************************************************************************* */ -TEST( dataSet, Balbianello) -{ - ///< The structure where we will save the SfM data - const string filename = findExampleDataFile("Balbianello"); - SfmData mydata; - CHECK(readBundler(filename, mydata)); - - // Check number of things - EXPECT_LONGS_EQUAL(5,mydata.nrCameras()); - EXPECT_LONGS_EQUAL(544,mydata.nrTracks()); - const SfmTrack& track0 = mydata.tracks[0]; - EXPECT_LONGS_EQUAL(3,track0.nrMeasurements()); - - // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); - const SfmCamera& camera0 = mydata.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; - EXPECT(assert_equal(expected,actual,1)); -} - /* ************************************************************************* */ TEST(dataSet, readG2o3D) { const string g2oFile = findExampleDataFile("pose3example"); @@ -461,160 +440,6 @@ TEST( dataSet, writeG2o3DNonDiagonalNoise) EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4)); } -/* ************************************************************************* */ -TEST( dataSet, readBAL_Dubrovnik) -{ - ///< The structure where we will save the SfM data - const string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData mydata; - CHECK(readBAL(filename, mydata)); - - // Check number of things - EXPECT_LONGS_EQUAL(3,mydata.nrCameras()); - EXPECT_LONGS_EQUAL(7,mydata.nrTracks()); - const SfmTrack& track0 = mydata.tracks[0]; - EXPECT_LONGS_EQUAL(3,track0.nrMeasurements()); - - // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); - const SfmCamera& camera0 = mydata.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; - EXPECT(assert_equal(expected,actual,12)); -} - -/* ************************************************************************* */ -TEST( dataSet, openGL2gtsam) -{ - Vector3 rotVec(0.2, 0.7, 1.1); - Rot3 R = Rot3::Expmap(rotVec); - Point3 t = Point3(0.0,0.0,0.0); - Pose3 poseGTSAM = Pose3(R,t); - - Pose3 expected = openGL2gtsam(R, t.x(), t.y(), t.z()); - - Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); //columns! - Rot3 cRw( - r1.x(), r2.x(), r3.x(), - -r1.y(), -r2.y(), -r3.y(), - -r1.z(), -r2.z(), -r3.z()); - Rot3 wRc = cRw.inverse(); - Pose3 actual = Pose3(wRc,t); - - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( dataSet, gtsam2openGL) -{ - Vector3 rotVec(0.2, 0.7, 1.1); - Rot3 R = Rot3::Expmap(rotVec); - Point3 t = Point3(1.0,20.0,10.0); - Pose3 actual = Pose3(R,t); - Pose3 poseGTSAM = openGL2gtsam(R, t.x(), t.y(), t.z()); - - Pose3 expected = gtsam2openGL(poseGTSAM); - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( dataSet, writeBAL_Dubrovnik) -{ - ///< Read a file using the unit tested readBAL - const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData readData; - readBAL(filenameToRead, readData); - - // Write readData to file filenameToWrite - const string filenameToWrite = createRewrittenFileName(filenameToRead); - CHECK(writeBAL(filenameToWrite, readData)); - - // Read what we wrote - SfmData writtenData; - CHECK(readBAL(filenameToWrite, writtenData)); - - // Check that what we read is the same as what we wrote - EXPECT_LONGS_EQUAL(readData.nrCameras(),writtenData.nrCameras()); - EXPECT_LONGS_EQUAL(readData.nrTracks(),writtenData.nrTracks()); - - for (size_t i = 0; i < readData.nrCameras(); i++){ - PinholeCamera expectedCamera = writtenData.cameras[i]; - PinholeCamera actualCamera = readData.cameras[i]; - EXPECT(assert_equal(expectedCamera,actualCamera)); - } - - for (size_t j = 0; j < readData.nrTracks(); j++){ - // check point - SfmTrack expectedTrack = writtenData.tracks[j]; - SfmTrack actualTrack = readData.tracks[j]; - Point3 expectedPoint = expectedTrack.p; - Point3 actualPoint = actualTrack.p; - EXPECT(assert_equal(expectedPoint,actualPoint)); - - // check rgb - Point3 expectedRGB = Point3( expectedTrack.r, expectedTrack.g, expectedTrack.b ); - Point3 actualRGB = Point3( actualTrack.r, actualTrack.g, actualTrack.b); - EXPECT(assert_equal(expectedRGB,actualRGB)); - - // check measurements - for (size_t k = 0; k < actualTrack.nrMeasurements(); k++){ - EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,actualTrack.measurements[k].first); - EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second)); - } - } -} - - -/* ************************************************************************* */ -TEST( dataSet, writeBALfromValues_Dubrovnik){ - - ///< Read a file using the unit tested readBAL - const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData readData; - readBAL(filenameToRead, readData); - - Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.3,0.1,0.3)); - - Values value; - for(size_t i=0; i < readData.nrCameras(); i++){ // for each camera - Pose3 pose = poseChange.compose(readData.cameras[i].pose()); - value.insert(X(i), pose); - } - for(size_t j=0; j < readData.nrTracks(); j++){ // for each point - Point3 point = poseChange.transformFrom( readData.tracks[j].p ); - value.insert(P(j), point); - } - - // Write values and readData to a file - const string filenameToWrite = createRewrittenFileName(filenameToRead); - writeBALfromValues(filenameToWrite, readData, value); - - // Read the file we wrote - SfmData writtenData; - readBAL(filenameToWrite, writtenData); - - // Check that the reprojection errors are the same and the poses are correct - // Check number of things - EXPECT_LONGS_EQUAL(3,writtenData.nrCameras()); - EXPECT_LONGS_EQUAL(7,writtenData.nrTracks()); - const SfmTrack& track0 = writtenData.tracks[0]; - EXPECT_LONGS_EQUAL(3,track0.nrMeasurements()); - - // Check projection of a given point - EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); - const SfmCamera& camera0 = writtenData.cameras[0]; - Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; - EXPECT(assert_equal(expected,actual,12)); - - Pose3 expectedPose = camera0.pose(); - Pose3 actualPose = value.at(X(0)); - EXPECT(assert_equal(expectedPose,actualPose, 1e-7)); - - Point3 expectedPoint = track0.p; - Point3 actualPoint = value.at(P(0)); - EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6)); -} - - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From ea5465cd84c7a12db820b1185403032aa2e1471a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Jan 2022 09:30:07 -0500 Subject: [PATCH 07/10] moved wrapping --- gtsam/sfm/sfm.i | 46 +++++++++++++++++++++++++++++++++++++++++++++- gtsam/slam/slam.i | 44 -------------------------------------------- 2 files changed, 45 insertions(+), 45 deletions(-) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 705892e60f..aaafa8e372 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -4,7 +4,51 @@ namespace gtsam { -// ##### +#include +class SfmTrack { + SfmTrack(); + SfmTrack(const gtsam::Point3& pt); + const Point3& point3() const; + + double r; + double g; + double b; + + std::vector> measurements; + + size_t nrMeasurements() const; + pair measurement(size_t idx) const; + pair siftIndex(size_t idx) const; + void addMeasurement(size_t idx, const gtsam::Point2& m); + + // enabling serialization functionality + void serialize() const; + + // enabling function to compare objects + bool equals(const gtsam::SfmTrack& expected, double tol) const; +}; + +#include +class SfmData { + SfmData(); + size_t nrCameras() const; + size_t nrTracks() const; + gtsam::PinholeCamera camera(size_t idx) const; + gtsam::SfmTrack track(size_t idx) const; + void addTrack(const gtsam::SfmTrack& t); + void addCamera(const gtsam::SfmCamera& cam); + + // enabling serialization functionality + void serialize() const; + + // enabling function to compare objects + bool equals(const gtsam::SfmData& expected, double tol) const; +}; + +gtsam::SfmData readBal(string filename); +bool writeBAL(string filename, gtsam::SfmData& data); +gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); +gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); #include diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 51a5904053..2785a3fb3d 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -209,50 +209,6 @@ virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor { #include -class SfmTrack { - SfmTrack(); - SfmTrack(const gtsam::Point3& pt); - const Point3& point3() const; - - double r; - double g; - double b; - - std::vector> measurements; - - size_t nrMeasurements() const; - pair measurement(size_t idx) const; - pair siftIndex(size_t idx) const; - void addMeasurement(size_t idx, const gtsam::Point2& m); - - // enabling serialization functionality - void serialize() const; - - // enabling function to compare objects - bool equals(const gtsam::SfmTrack& expected, double tol) const; -}; - -class SfmData { - SfmData(); - size_t nrCameras() const; - size_t nrTracks() const; - gtsam::PinholeCamera camera(size_t idx) const; - gtsam::SfmTrack track(size_t idx) const; - void addTrack(const gtsam::SfmTrack& t); - void addCamera(const gtsam::SfmCamera& cam); - - // enabling serialization functionality - void serialize() const; - - // enabling function to compare objects - bool equals(const gtsam::SfmData& expected, double tol) const; -}; - -gtsam::SfmData readBal(string filename); -bool writeBAL(string filename, gtsam::SfmData& data); -gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); -gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); - enum NoiseFormat { NoiseFormatG2O, NoiseFormatTORO, From 34b8e4e13039a53b99336009ff14aa1d9cc9df0c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Jan 2022 12:23:31 -0500 Subject: [PATCH 08/10] fix CI issue --- gtsam/sfm/SfmTrack.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/SfmTrack.cpp b/gtsam/sfm/SfmTrack.cpp index 11f104c09e..e97d5a405d 100644 --- a/gtsam/sfm/SfmTrack.cpp +++ b/gtsam/sfm/SfmTrack.cpp @@ -27,7 +27,7 @@ void SfmTrack::print(const std::string& s) const { std::cout << " measurements of point " << p << std::endl; } -bool SfmTrack::equals(const SfmTrack& sfmTrack, double tolÏ) const { +bool SfmTrack::equals(const SfmTrack& sfmTrack, double tol) const { // check the 3D point if (!p.isApprox(sfmTrack.p)) { return false; From a4fc68f759a9a790b8d614ef11e9c37b99feeaf4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Jan 2022 12:27:12 -0500 Subject: [PATCH 09/10] Fix DiscreteBayesTree test --- .../discrete/tests/testDiscreteBayesTree.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp index 26356be3d8..6635633a27 100644 --- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp @@ -243,27 +243,27 @@ TEST(DiscreteBayesTree, Dot) { string actual = self.bayesTree->dot(); EXPECT(actual == "digraph G{\n" - "0[label=\"13,11,6,7\"];\n" + "0[label=\"13, 11, 6, 7\"];\n" "0->1\n" - "1[label=\"14 : 11,13\"];\n" + "1[label=\"14 : 11, 13\"];\n" "1->2\n" - "2[label=\"9,12 : 14\"];\n" + "2[label=\"9, 12 : 14\"];\n" "2->3\n" - "3[label=\"3 : 9,12\"];\n" + "3[label=\"3 : 9, 12\"];\n" "2->4\n" - "4[label=\"2 : 9,12\"];\n" + "4[label=\"2 : 9, 12\"];\n" "2->5\n" - "5[label=\"8 : 12,14\"];\n" + "5[label=\"8 : 12, 14\"];\n" "5->6\n" - "6[label=\"1 : 8,12\"];\n" + "6[label=\"1 : 8, 12\"];\n" "5->7\n" - "7[label=\"0 : 8,12\"];\n" + "7[label=\"0 : 8, 12\"];\n" "1->8\n" - "8[label=\"10 : 13,14\"];\n" + "8[label=\"10 : 13, 14\"];\n" "8->9\n" - "9[label=\"5 : 10,13\"];\n" + "9[label=\"5 : 10, 13\"];\n" "8->10\n" - "10[label=\"4 : 10,13\"];\n" + "10[label=\"4 : 10, 13\"];\n" "}"); } From 762e8097bb73df95c3d23ef0ac33adc9179c2216 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Jan 2022 12:46:42 -0500 Subject: [PATCH 10/10] nr -> number --- examples/SFMExampleExpressions_bal.cpp | 2 +- examples/SFMExample_bal.cpp | 2 +- examples/SFMExample_bal_COLAMD_METIS.cpp | 4 +- gtsam/sfm/SfmData.cpp | 43 ++++++++++--------- gtsam/sfm/SfmData.h | 4 +- gtsam/sfm/SfmTrack.cpp | 4 +- gtsam/sfm/SfmTrack.h | 2 +- gtsam/sfm/ShonanAveraging.h | 2 +- gtsam/sfm/sfm.i | 10 ++--- gtsam/sfm/tests/testSfmData.cpp | 32 +++++++------- gtsam/slam/SmartFactorBase.h | 2 +- .../slam/tests/testEssentialMatrixFactor.cpp | 6 +-- python/gtsam/examples/SFMExample_bal.py | 16 +++---- python/gtsam/tests/test_SfmData.py | 4 +- tests/testGeneralSFMFactorB.cpp | 2 +- timing/timeSFMBAL.cpp | 2 +- timing/timeSFMBAL.h | 4 +- timing/timeSFMBALautodiff.cpp | 2 +- timing/timeSFMBALcamTnav.cpp | 2 +- timing/timeSFMBALnavTcam.cpp | 2 +- timing/timeSFMBALsmart.cpp | 2 +- 21 files changed, 75 insertions(+), 74 deletions(-) diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp index 4cc6b1185d..4fd6c1ada2 100644 --- a/examples/SFMExampleExpressions_bal.cpp +++ b/examples/SFMExampleExpressions_bal.cpp @@ -49,7 +49,7 @@ int main(int argc, char* argv[]) { SfmData mydata; readBAL(filename, mydata); cout << boost::format("read %1% tracks on %2% cameras\n") % - mydata.nrTracks() % mydata.nrCameras(); + mydata.numberTracks() % mydata.numberCameras(); // Create a factor graph ExpressionFactorGraph graph; diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index a93e438508..9d6e38a153 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -43,7 +43,7 @@ int main (int argc, char* argv[]) { // Load the SfM data from file SfmData mydata; readBAL(filename, mydata); - cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.nrTracks() % mydata.nrCameras(); + cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras(); // Create a factor graph NonlinearFactorGraph graph; diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 90b397b06b..b59510c9fa 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -48,7 +48,7 @@ int main(int argc, char* argv[]) { SfmData mydata; readBAL(filename, mydata); cout << boost::format("read %1% tracks on %2% cameras\n") % - mydata.nrTracks() % mydata.nrCameras(); + mydata.numberTracks() % mydata.numberCameras(); // Create a factor graph NonlinearFactorGraph graph; @@ -131,7 +131,7 @@ int main(int argc, char* argv[]) { cout << "Time comparison by solving " << filename << " results:" << endl; cout << boost::format("%1% point tracks and %2% cameras\n") % - mydata.nrTracks() % mydata.nrCameras() + mydata.numberTracks() % mydata.numberCameras() << endl; tictoc_print_(); diff --git a/gtsam/sfm/SfmData.cpp b/gtsam/sfm/SfmData.cpp index 1e3d53601e..2f5396fcd1 100644 --- a/gtsam/sfm/SfmData.cpp +++ b/gtsam/sfm/SfmData.cpp @@ -32,26 +32,27 @@ using gtsam::symbol_shorthand::X; /* ************************************************************************** */ void SfmData::print(const std::string &s) const { - std::cout << "Number of cameras = " << nrCameras() << std::endl; - std::cout << "Number of tracks = " << nrTracks() << std::endl; + std::cout << "Number of cameras = " << cameras.size() << std::endl; + std::cout << "Number of tracks = " << tracks.size() << std::endl; } /* ************************************************************************** */ bool SfmData::equals(const SfmData &sfmData, double tol) const { // check number of cameras and tracks - if (nrCameras() != sfmData.nrCameras() || nrTracks() != sfmData.nrTracks()) { + if (cameras.size() != sfmData.cameras.size() || + tracks.size() != sfmData.tracks.size()) { return false; } // check each camera - for (size_t i = 0; i < nrCameras(); ++i) { + for (size_t i = 0; i < cameras.size(); ++i) { if (!camera(i).equals(sfmData.camera(i), tol)) { return false; } } // check each track - for (size_t j = 0; j < nrTracks(); ++j) { + for (size_t j = 0; j < tracks.size(); ++j) { if (!track(j).equals(sfmData.track(j), tol)) { return false; } @@ -264,19 +265,19 @@ bool writeBAL(const std::string &filename, SfmData &data) { // Write the number of camera poses and 3D points size_t nrObservations = 0; - for (size_t j = 0; j < data.nrTracks(); j++) { - nrObservations += data.tracks[j].nrMeasurements(); + for (size_t j = 0; j < data.tracks.size(); j++) { + nrObservations += data.tracks[j].numberMeasurements(); } // Write observations - os << data.nrCameras() << " " << data.nrTracks() << " " << nrObservations - << endl; + os << data.cameras.size() << " " << data.tracks.size() << " " + << nrObservations << endl; os << endl; - for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j + for (size_t j = 0; j < data.tracks.size(); j++) { // for each 3D point j const SfmTrack &track = data.tracks[j]; - for (size_t k = 0; k < track.nrMeasurements(); + for (size_t k = 0; k < track.numberMeasurements(); k++) { // for each observation of the 3D point j size_t i = track.measurements[k].first; // camera id double u0 = data.cameras[i].calibration().px(); @@ -301,7 +302,7 @@ bool writeBAL(const std::string &filename, SfmData &data) { os << endl; // Write cameras - for (size_t i = 0; i < data.nrCameras(); i++) { // for each camera + for (size_t i = 0; i < data.cameras.size(); i++) { // for each camera Pose3 poseGTSAM = data.cameras[i].pose(); Cal3Bundler cameraCalibration = data.cameras[i].calibration(); Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); @@ -316,7 +317,7 @@ bool writeBAL(const std::string &filename, SfmData &data) { } // Write the points - for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j + for (size_t j = 0; j < data.tracks.size(); j++) { // for each 3D point j Point3 point = data.tracks[j].p; os << point.x() << endl; os << point.y() << endl; @@ -336,8 +337,8 @@ bool writeBALfromValues(const std::string &filename, const SfmData &data, // Store poses or cameras in SfmData size_t nrPoses = values.count(); - if (nrPoses == dataValues.nrCameras()) { // we only estimated camera poses - for (size_t i = 0; i < dataValues.nrCameras(); i++) { // for each camera + 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)); Cal3Bundler K = dataValues.cameras[i].calibration(); Camera camera(pose, K); @@ -345,24 +346,24 @@ bool writeBALfromValues(const std::string &filename, const SfmData &data, } } else { size_t nrCameras = values.count(); - if (nrCameras == dataValues.nrCameras()) { // we only estimated camera - // poses and calibration - for (size_t i = 0; i < nrCameras; i++) { // for each camera - Key cameraKey = i; // symbol('c',i); + if (nrCameras == dataValues.cameras.size()) { // we only estimated camera + // poses and calibration + for (size_t i = 0; i < nrCameras; i++) { // for each camera + Key cameraKey = i; // symbol('c',i); Camera camera = values.at(cameraKey); dataValues.cameras[i] = camera; } } else { cout << "writeBALfromValues: different number of cameras in " "SfM_dataValues (#cameras " - << dataValues.nrCameras() << ") and values (#cameras " << nrPoses + << dataValues.cameras.size() << ") and values (#cameras " << nrPoses << ", #poses " << nrCameras << ")!!" << endl; return false; } } // Store 3D points in SfmData - size_t nrPoints = values.count(), nrTracks = dataValues.nrTracks(); + size_t nrPoints = values.count(), nrTracks = dataValues.tracks.size(); if (nrPoints != nrTracks) { cout << "writeBALfromValues: different number of points in " "SfM_dataValues (#points= " diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h index 8770fcd4a2..e2a6985f20 100644 --- a/gtsam/sfm/SfmData.h +++ b/gtsam/sfm/SfmData.h @@ -50,10 +50,10 @@ struct SfmData { void addCamera(const SfmCamera& cam) { cameras.push_back(cam); } /// The number of reconstructed 3D points - size_t nrTracks() const { return tracks.size(); } + size_t numberTracks() const { return tracks.size(); } /// The number of cameras - size_t nrCameras() const { return cameras.size(); } + size_t numberCameras() const { return cameras.size(); } /// The track formed by series of landmark measurements SfmTrack track(size_t idx) const { return tracks[idx]; } diff --git a/gtsam/sfm/SfmTrack.cpp b/gtsam/sfm/SfmTrack.cpp index e97d5a405d..d571e7c359 100644 --- a/gtsam/sfm/SfmTrack.cpp +++ b/gtsam/sfm/SfmTrack.cpp @@ -39,13 +39,13 @@ bool SfmTrack::equals(const SfmTrack& sfmTrack, double tol) const { } // compare size of vectors for measurements and siftIndices - if (nrMeasurements() != sfmTrack.nrMeasurements() || + if (numberMeasurements() != sfmTrack.numberMeasurements() || siftIndices.size() != sfmTrack.siftIndices.size()) { return false; } // compare measurements (order sensitive) - for (size_t idx = 0; idx < nrMeasurements(); ++idx) { + for (size_t idx = 0; idx < numberMeasurements(); ++idx) { SfmMeasurement measurement = measurements[idx]; SfmMeasurement otherMeasurement = sfmTrack.measurements[idx]; diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h index 35603a6f36..ee9128d04b 100644 --- a/gtsam/sfm/SfmTrack.h +++ b/gtsam/sfm/SfmTrack.h @@ -68,7 +68,7 @@ struct SfmTrack { } /// Total number of measurements in this track - size_t nrMeasurements() const { return measurements.size(); } + size_t numberMeasurements() const { return measurements.size(); } /// Get the measurement (camera index, Point2) at pose index `idx` const SfmMeasurement& measurement(size_t idx) const { diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index de12de4782..a5c31534c3 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -165,7 +165,7 @@ class GTSAM_EXPORT ShonanAveraging { size_t nrUnknowns() const { return nrUnknowns_; } /// Return number of measurements - size_t nrMeasurements() const { return measurements_.size(); } + size_t numberMeasurements() const { return measurements_.size(); } /// k^th binary measurement const BinaryMeasurement &measurement(size_t k) const { diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index aaafa8e372..14f183d169 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -16,7 +16,7 @@ class SfmTrack { std::vector> measurements; - size_t nrMeasurements() const; + size_t numberMeasurements() const; pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; void addMeasurement(size_t idx, const gtsam::Point2& m); @@ -31,8 +31,8 @@ class SfmTrack { #include class SfmData { SfmData(); - size_t nrCameras() const; - size_t nrTracks() const; + size_t numberCameras() const; + size_t numberTracks() const; gtsam::PinholeCamera camera(size_t idx) const; gtsam::SfmTrack track(size_t idx) const; void addTrack(const gtsam::SfmTrack& t); @@ -136,7 +136,7 @@ class ShonanAveraging2 { // Query properties size_t nrUnknowns() const; - size_t nrMeasurements() const; + size_t numberMeasurements() const; gtsam::Rot2 measured(size_t i); gtsam::KeyVector keys(size_t i); @@ -184,7 +184,7 @@ class ShonanAveraging3 { // Query properties size_t nrUnknowns() const; - size_t nrMeasurements() const; + size_t numberMeasurements() const; gtsam::Rot3 measured(size_t i); gtsam::KeyVector keys(size_t i); diff --git a/gtsam/sfm/tests/testSfmData.cpp b/gtsam/sfm/tests/testSfmData.cpp index f19542c260..bc1c8b645d 100644 --- a/gtsam/sfm/tests/testSfmData.cpp +++ b/gtsam/sfm/tests/testSfmData.cpp @@ -39,10 +39,10 @@ TEST(dataSet, Balbianello) { CHECK(readBundler(filename, mydata)); // Check number of things - EXPECT_LONGS_EQUAL(5, mydata.nrCameras()); - EXPECT_LONGS_EQUAL(544, mydata.nrTracks()); + EXPECT_LONGS_EQUAL(5, mydata.numberCameras()); + EXPECT_LONGS_EQUAL(544, mydata.numberTracks()); const SfmTrack& track0 = mydata.tracks[0]; - EXPECT_LONGS_EQUAL(3, track0.nrMeasurements()); + EXPECT_LONGS_EQUAL(3, track0.numberMeasurements()); // Check projection of a given point EXPECT_LONGS_EQUAL(0, track0.measurements[0].first); @@ -60,10 +60,10 @@ TEST(dataSet, readBAL_Dubrovnik) { CHECK(readBAL(filename, mydata)); // Check number of things - EXPECT_LONGS_EQUAL(3, mydata.nrCameras()); - EXPECT_LONGS_EQUAL(7, mydata.nrTracks()); + EXPECT_LONGS_EQUAL(3, mydata.numberCameras()); + EXPECT_LONGS_EQUAL(7, mydata.numberTracks()); const SfmTrack& track0 = mydata.tracks[0]; - EXPECT_LONGS_EQUAL(3, track0.nrMeasurements()); + EXPECT_LONGS_EQUAL(3, track0.numberMeasurements()); // Check projection of a given point EXPECT_LONGS_EQUAL(0, track0.measurements[0].first); @@ -119,16 +119,16 @@ TEST(dataSet, writeBAL_Dubrovnik) { CHECK(readBAL(filenameToWrite, writtenData)); // Check that what we read is the same as what we wrote - EXPECT_LONGS_EQUAL(readData.nrCameras(), writtenData.nrCameras()); - EXPECT_LONGS_EQUAL(readData.nrTracks(), writtenData.nrTracks()); + EXPECT_LONGS_EQUAL(readData.numberCameras(), writtenData.numberCameras()); + EXPECT_LONGS_EQUAL(readData.numberTracks(), writtenData.numberTracks()); - for (size_t i = 0; i < readData.nrCameras(); i++) { + for (size_t i = 0; i < readData.numberCameras(); i++) { PinholeCamera expectedCamera = writtenData.cameras[i]; PinholeCamera actualCamera = readData.cameras[i]; EXPECT(assert_equal(expectedCamera, actualCamera)); } - for (size_t j = 0; j < readData.nrTracks(); j++) { + for (size_t j = 0; j < readData.numberTracks(); j++) { // check point SfmTrack expectedTrack = writtenData.tracks[j]; SfmTrack actualTrack = readData.tracks[j]; @@ -143,7 +143,7 @@ TEST(dataSet, writeBAL_Dubrovnik) { EXPECT(assert_equal(expectedRGB, actualRGB)); // check measurements - for (size_t k = 0; k < actualTrack.nrMeasurements(); k++) { + for (size_t k = 0; k < actualTrack.numberMeasurements(); k++) { EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first, actualTrack.measurements[k].first); EXPECT(assert_equal(expectedTrack.measurements[k].second, @@ -163,11 +163,11 @@ TEST(dataSet, writeBALfromValues_Dubrovnik) { Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.3, 0.1, 0.3)); Values value; - for (size_t i = 0; i < readData.nrCameras(); i++) { // for each camera + 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); } - for (size_t j = 0; j < readData.nrTracks(); j++) { // for each point + 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); } @@ -182,10 +182,10 @@ TEST(dataSet, writeBALfromValues_Dubrovnik) { // Check that the reprojection errors are the same and the poses are correct // Check number of things - EXPECT_LONGS_EQUAL(3, writtenData.nrCameras()); - EXPECT_LONGS_EQUAL(7, writtenData.nrTracks()); + EXPECT_LONGS_EQUAL(3, writtenData.numberCameras()); + EXPECT_LONGS_EQUAL(7, writtenData.numberTracks()); const SfmTrack& track0 = writtenData.tracks[0]; - EXPECT_LONGS_EQUAL(3, track0.nrMeasurements()); + EXPECT_LONGS_EQUAL(3, track0.numberMeasurements()); // Check projection of a given point EXPECT_LONGS_EQUAL(0, track0.measurements[0].first); diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 42a9508481..16712c429f 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -146,7 +146,7 @@ class GTSAM_EXPORT SmartFactorBase: public NonlinearFactor { */ template void add(const SFM_TRACK& trackToAdd) { - for (size_t k = 0; k < trackToAdd.nrMeasurements(); k++) { + for (size_t k = 0; k < trackToAdd.numberMeasurements(); k++) { this->measured_.push_back(trackToAdd.measurements[k].second); this->keys_.push_back(trackToAdd.measurements[k].first); } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 6df59ba489..1a5c64c8c2 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -632,14 +632,14 @@ TEST(EssentialMatrixFactor2, extraMinimization) { // We start with a factor graph and add constraints to it // Noise sigma is 1, assuming pixel measurements NonlinearFactorGraph graph; - for (size_t i = 0; i < data.nrTracks(); i++) + for (size_t i = 0; i < data.numberTracks(); i++) graph.emplace_shared(100, i, pA(i), pB(i), model2, K); // Check error at ground truth Values truth; truth.insert(100, trueE); - for (size_t i = 0; i < data.nrTracks(); i++) { + for (size_t i = 0; i < data.numberTracks(); i++) { Point3 P1 = data.tracks[i].p; truth.insert(i, double(baseline / P1.z())); } @@ -654,7 +654,7 @@ TEST(EssentialMatrixFactor2, extraMinimization) { // Check result EssentialMatrix actual = result.at(100); EXPECT(assert_equal(trueE, actual, 1e-1)); - for (size_t i = 0; i < data.nrTracks(); i++) + for (size_t i = 0; i < data.numberTracks(); i++) EXPECT_DOUBLES_EQUAL(truth.at(i), result.at(i), 1e-1); // Check error at result diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 1c4d76c837..8db1c2cb40 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -29,10 +29,10 @@ def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None: """Plot the SFM results.""" plot_vals = gtsam.Values() - for cam_idx in range(scene_data.nrCameras()): + for cam_idx in range(scene_data.numberCameras()): plot_vals.insert(C(cam_idx), result.atPinholeCameraCal3Bundler(C(cam_idx)).pose()) - for j in range(scene_data.nrTracks()): + for j in range(scene_data.numberTracks()): plot_vals.insert(P(j), result.atPoint3(P(j))) plot.plot_3d_points(0, plot_vals, linespec="g.") @@ -47,8 +47,8 @@ def run(args: argparse.Namespace) -> None: # Load the SfM data from file scene_data = gtsam.readBal(input_file) - logging.info("read %d tracks on %d cameras\n", scene_data.nrTracks(), - scene_data.nrCameras()) + logging.info("read %d tracks on %d cameras\n", scene_data.numberTracks(), + scene_data.numberCameras()) # Create a factor graph graph = gtsam.NonlinearFactorGraph() @@ -57,10 +57,10 @@ def run(args: argparse.Namespace) -> None: noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v # Add measurements to the factor graph - for j in range(scene_data.nrTracks()): + for j in range(scene_data.numberTracks()): track = scene_data.track(j) # SfmTrack # retrieve the SfmMeasurement objects - for m_idx in range(track.nrMeasurements()): + for m_idx in range(track.numberMeasurements()): # 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 @@ -82,13 +82,13 @@ def run(args: argparse.Namespace) -> None: i = 0 # add each PinholeCameraCal3Bundler - for cam_idx in range(scene_data.nrCameras()): + for cam_idx in range(scene_data.numberCameras()): camera = scene_data.camera(cam_idx) initial.insert(C(i), camera) i += 1 # add each SfmTrack - for j in range(scene_data.nrTracks()): + for j in range(scene_data.numberTracks()): track = scene_data.track(j) initial.insert(P(j), track.point3()) diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py index 79004f03c9..4a45f91ba2 100644 --- a/python/gtsam/tests/test_SfmData.py +++ b/python/gtsam/tests/test_SfmData.py @@ -42,7 +42,7 @@ def test_tracks(self): self.tracks.addMeasurement(i1, uv_i1) self.tracks.addMeasurement(i2, uv_i2) # Number of measurements in the track is 2 - self.assertEqual(self.tracks.nrMeasurements(), 2) + self.assertEqual(self.tracks.numberMeasurements(), 2) # camera_idx in the first measurement of the track corresponds to i1 cam_idx, img_measurement = self.tracks.measurement(0) self.assertEqual(cam_idx, i1) @@ -70,7 +70,7 @@ def test_data(self): self.data.addTrack(self.tracks) self.data.addTrack(track2) # Number of tracks in SfmData is 2 - self.assertEqual(self.data.nrTracks(), 2) + self.assertEqual(self.data.numberTracks(), 2) # camera idx of first measurement of second track corresponds to i1 cam_idx, img_measurement = self.data.track(1).measurement(0) self.assertEqual(cam_idx, i1) diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp index 895c8484ba..0bf609adc5 100644 --- a/tests/testGeneralSFMFactorB.cpp +++ b/tests/testGeneralSFMFactorB.cpp @@ -49,7 +49,7 @@ TEST(PinholeCamera, BAL) { SharedNoiseModel unit2 = noiseModel::Unit::Create(2); NonlinearFactorGraph graph; - for (size_t j = 0; j < db.nrTracks(); j++) { + for (size_t j = 0; j < db.numberTracks(); j++) { for (const SfmMeasurement& m: db.tracks[j].measurements) graph.emplace_shared(m.second, unit2, m.first, P(j)); } diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 06b9637572..c1f36abd06 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -36,7 +36,7 @@ int main(int argc, char* argv[]) { // Build graph using conventional GeneralSFMFactor NonlinearFactorGraph graph; - for (size_t j = 0; j < db.nrTracks(); j++) { + for (size_t j = 0; j < db.numberTracks(); j++) { for (const SfmMeasurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index 3108624ce5..347500cd2b 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -73,8 +73,8 @@ int optimize(const SfmData& db, const NonlinearFactorGraph& graph, if (gUseSchur) { // Create Schur-complement ordering Ordering ordering; - for (size_t j = 0; j < db.nrTracks(); j++) ordering.push_back(P(j)); - for (size_t i = 0; i < db.nrCameras(); i++) { + for (size_t j = 0; j < db.numberTracks(); j++) ordering.push_back(P(j)); + for (size_t i = 0; i < db.numberCameras(); i++) { ordering.push_back(C(i)); if (separateCalibration) ordering.push_back(K(i)); } diff --git a/timing/timeSFMBALautodiff.cpp b/timing/timeSFMBALautodiff.cpp index 63e1931459..083d367292 100644 --- a/timing/timeSFMBALautodiff.cpp +++ b/timing/timeSFMBALautodiff.cpp @@ -44,7 +44,7 @@ int main(int argc, char* argv[]) { // Build graph NonlinearFactorGraph graph; - for (size_t j = 0; j < db.nrTracks(); j++) { + for (size_t j = 0; j < db.numberTracks(); j++) { for (const SfmMeasurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; diff --git a/timing/timeSFMBALcamTnav.cpp b/timing/timeSFMBALcamTnav.cpp index cc26710403..a564a3a350 100644 --- a/timing/timeSFMBALcamTnav.cpp +++ b/timing/timeSFMBALcamTnav.cpp @@ -33,7 +33,7 @@ int main(int argc, char* argv[]) { // Build graph using conventional GeneralSFMFactor NonlinearFactorGraph graph; - for (size_t j = 0; j < db.nrTracks(); j++) { + for (size_t j = 0; j < db.numberTracks(); j++) { for (const SfmMeasurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; diff --git a/timing/timeSFMBALnavTcam.cpp b/timing/timeSFMBALnavTcam.cpp index 4c3bc06214..5299c85523 100644 --- a/timing/timeSFMBALnavTcam.cpp +++ b/timing/timeSFMBALnavTcam.cpp @@ -33,7 +33,7 @@ int main(int argc, char* argv[]) { // Build graph using conventional GeneralSFMFactor NonlinearFactorGraph graph; - for (size_t j = 0; j < db.nrTracks(); j++) { + for (size_t j = 0; j < db.numberTracks(); j++) { Point3_ nav_point_(P(j)); for (const SfmMeasurement& m: db.tracks[j].measurements) { size_t i = m.first; diff --git a/timing/timeSFMBALsmart.cpp b/timing/timeSFMBALsmart.cpp index f37f35a394..fe2f7b925d 100644 --- a/timing/timeSFMBALsmart.cpp +++ b/timing/timeSFMBALsmart.cpp @@ -35,7 +35,7 @@ int main(int argc, char* argv[]) { // Add smart factors to graph NonlinearFactorGraph graph; - for (size_t j = 0; j < db.nrTracks(); j++) { + for (size_t j = 0; j < db.numberTracks(); j++) { auto smartFactor = boost::make_shared(gNoiseModel); for (const SfmMeasurement& m : db.tracks[j].measurements) { size_t i = m.first;