diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 36a73b792..2b6f37a45 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -85,6 +85,7 @@ class BinaryMeasurement { typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; +typedef gtsam::BinaryMeasurement BinaryMeasurementPoint3; class BinaryMeasurementsUnit3 { BinaryMeasurementsUnit3(); @@ -93,6 +94,13 @@ class BinaryMeasurementsUnit3 { void push_back(const gtsam::BinaryMeasurement& measurement); }; +class BinaryMeasurementsPoint3 { + BinaryMeasurementsPoint3(); + size_t size() const; + gtsam::BinaryMeasurement at(size_t idx) const; + void push_back(const gtsam::BinaryMeasurement& measurement); +}; + #include // TODO(frank): copy/pasta below until we have integer template paremeters in @@ -254,23 +262,23 @@ class MFAS { }; #include +class TranslationRecoveryParams { + gtsam::BinaryMeasurementsPoint3 getBetweenTranslations() const; + gtsam::Values getInitialValues() const; + gtsam::LevenbergMarquardtParams getLMParams() const; + + void setBetweenTranslations( + const gtsam::BinaryMeasurementsPoint3& betweenTranslations); + void setInitialValues(const gtsam::Values& values); + void setLMParams(const gtsam::LevenbergMarquardtParams& lmParams); +}; + class TranslationRecovery { TranslationRecovery( const gtsam::BinaryMeasurementsUnit3& relativeTranslations, - const gtsam::LevenbergMarquardtParams& lmParams); - TranslationRecovery( - const gtsam::BinaryMeasurementsUnit3& - relativeTranslations); // default LevenbergMarquardtParams - gtsam::NonlinearFactorGraph buildGraph() const; - gtsam::Values initializeRandomly() const; - void addPrior(gtsam::Key i, const gtsam::Point3& prior, - gtsam::NonlinearFactorGraph* graph, - const gtsam::SharedNoiseModel& model = - gtsam::noiseModel::Isotropic::Sigma(3, 0.01)) const; - void addRelativeHardConstraint(gtsam::Key i, gtsam::Key j, - const gtsam::Point3& w_itj, - gtsam::NonlinearFactorGraph* graph) const; - gtsam::Values addDuplicateNodes(const gtsam::Values& result) const; + const gtsam::TranslationRecoveryParams& lmParams); + TranslationRecovery(const gtsam::BinaryMeasurementsUnit3& + relativeTranslations); // default params gtsam::Values run(const double scale) const; gtsam::Values run() const; // default scale = 1.0 }; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index dc450a9f7..247be5bae 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -223,6 +223,7 @@ parse3DFactors(const std::string &filename, size_t maxIndex = 0); using BinaryMeasurementsUnit3 = std::vector>; +using BinaryMeasurementsPoint3 = std::vector>; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 inline boost::optional GTSAM_DEPRECATED diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index f5869b145..6c0c6e83a 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -47,6 +47,7 @@ set(ignore gtsam::Pose3Vector gtsam::Rot3Vector gtsam::KeyVector + gtsam::BinaryMeasurementsPoint3 gtsam::BinaryMeasurementsUnit3 gtsam::DiscreteKey gtsam::KeyPairDoubleMap) @@ -124,6 +125,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::Pose3Vector gtsam::KeyVector gtsam::FixedLagSmootherKeyTimestampMapValue + gtsam::BinaryMeasurementsPoint3 gtsam::BinaryMeasurementsUnit3 gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3Bundler diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 6de15217f..7a5084d4a 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -11,6 +11,8 @@ * and saves one copy operation. */ +py::bind_vector > >( + m_, "BinaryMeasurementsPoint3"); py::bind_vector > >( m_, "BinaryMeasurementsUnit3"); py::bind_map(m_, "KeyPairDoubleMap");