wrapper changes

release/4.3a0
Akshay Krishnan 2022-05-05 18:52:01 -07:00
parent 1d6fd5409a
commit 279b9bedf9
4 changed files with 27 additions and 14 deletions

View File

@ -85,6 +85,7 @@ class BinaryMeasurement {
typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3; typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3; typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
typedef gtsam::BinaryMeasurement<gtsam::Point3> BinaryMeasurementPoint3;
class BinaryMeasurementsUnit3 { class BinaryMeasurementsUnit3 {
BinaryMeasurementsUnit3(); BinaryMeasurementsUnit3();
@ -93,6 +94,13 @@ class BinaryMeasurementsUnit3 {
void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement); void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
}; };
class BinaryMeasurementsPoint3 {
BinaryMeasurementsPoint3();
size_t size() const;
gtsam::BinaryMeasurement<gtsam::Point3> at(size_t idx) const;
void push_back(const gtsam::BinaryMeasurement<gtsam::Point3>& measurement);
};
#include <gtsam/sfm/ShonanAveraging.h> #include <gtsam/sfm/ShonanAveraging.h>
// TODO(frank): copy/pasta below until we have integer template paremeters in // TODO(frank): copy/pasta below until we have integer template paremeters in
@ -254,23 +262,23 @@ class MFAS {
}; };
#include <gtsam/sfm/TranslationRecovery.h> #include <gtsam/sfm/TranslationRecovery.h>
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 { class TranslationRecovery {
TranslationRecovery( TranslationRecovery(
const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
const gtsam::LevenbergMarquardtParams& lmParams); const gtsam::TranslationRecoveryParams& lmParams);
TranslationRecovery( TranslationRecovery(const gtsam::BinaryMeasurementsUnit3&
const gtsam::BinaryMeasurementsUnit3& relativeTranslations); // default params
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;
gtsam::Values run(const double scale) const; gtsam::Values run(const double scale) const;
gtsam::Values run() const; // default scale = 1.0 gtsam::Values run() const; // default scale = 1.0
}; };

View File

@ -223,6 +223,7 @@ parse3DFactors(const std::string &filename,
size_t maxIndex = 0); size_t maxIndex = 0);
using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>; using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
using BinaryMeasurementsPoint3 = std::vector<BinaryMeasurement<Point3>>;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
inline boost::optional<IndexedPose> GTSAM_DEPRECATED inline boost::optional<IndexedPose> GTSAM_DEPRECATED

View File

@ -47,6 +47,7 @@ set(ignore
gtsam::Pose3Vector gtsam::Pose3Vector
gtsam::Rot3Vector gtsam::Rot3Vector
gtsam::KeyVector gtsam::KeyVector
gtsam::BinaryMeasurementsPoint3
gtsam::BinaryMeasurementsUnit3 gtsam::BinaryMeasurementsUnit3
gtsam::DiscreteKey gtsam::DiscreteKey
gtsam::KeyPairDoubleMap) gtsam::KeyPairDoubleMap)
@ -124,6 +125,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
gtsam::Pose3Vector gtsam::Pose3Vector
gtsam::KeyVector gtsam::KeyVector
gtsam::FixedLagSmootherKeyTimestampMapValue gtsam::FixedLagSmootherKeyTimestampMapValue
gtsam::BinaryMeasurementsPoint3
gtsam::BinaryMeasurementsUnit3 gtsam::BinaryMeasurementsUnit3
gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3_S2
gtsam::CameraSetCal3Bundler gtsam::CameraSetCal3Bundler

View File

@ -11,6 +11,8 @@
* and saves one copy operation. * and saves one copy operation.
*/ */
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Point3> > >(
m_, "BinaryMeasurementsPoint3");
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >( py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
m_, "BinaryMeasurementsUnit3"); m_, "BinaryMeasurementsUnit3");
py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap"); py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");