12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wrapper updates

release/4.3a0
Akshay Krishnan 2022-05-07 18:28:54 -07:00
parent 230bb8eb11
commit c0e5ce9ef9
3 changed files with 30 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
gtsam/sfm/sfm.i
View File

@ -265,19 +265,33 @@ class MFAS {
class TranslationRecoveryParams { class TranslationRecoveryParams {
gtsam::LevenbergMarquardtParams lmParams; gtsam::LevenbergMarquardtParams lmParams;
gtsam::Values initial; gtsam::Values initial;
TranslationRecoveryParams();
}; };
class TranslationRecovery { class TranslationRecovery {
TranslationRecovery( TranslationRecovery(const gtsam::TranslationRecoveryParams& lmParams);
TranslationRecovery(); // default params.
void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
const double scale,
const gtsam::BinaryMeasurementsPoint3& betweenTranslations,
gtsam::NonlinearFactorGraph @graph,
const gtsam::SharedNoiseModel& priorNoiseModel) const;
void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
const double scale,
const gtsam::BinaryMeasurementsPoint3& betweenTranslations,
gtsam::NonlinearFactorGraph @graph) const;
gtsam::NonlinearFactorGraph buildGraph(
const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const;
gtsam::Values run(
const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
const gtsam::TranslationRecoveryParams& lmParams); const double scale,
TranslationRecovery(const gtsam::BinaryMeasurementsUnit3& const gtsam::BinaryMeasurementsPoint3& betweenTranslations) const;
relativeTranslations); // default params
gtsam::Values run(const gtsam::BinaryMeasurementsPoint3& betweenTranslations,
const double scale) const;
// default empty betweenTranslations // default empty betweenTranslations
// gtsam::Values run(const double scale) const; gtsam::Values run(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
gtsam::Values run() const; // default scale = 1.0 const double scale) const;
// default scale = 1.0, empty betweenTranslations
gtsam::Values run(
const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const;
}; };
} // namespace gtsam } // namespace gtsam

2
python/gtsam/examples/TranslationAveragingExample.py
View File

@ -123,7 +123,7 @@ def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3,
w_iZj_inliers = filter_outliers(w_iZj_list) w_iZj_inliers = filter_outliers(w_iZj_list)
# Run the optimizer to obtain translations for normalized directions. # Run the optimizer to obtain translations for normalized directions.
wtc_values = gtsam.TranslationRecovery(w_iZj_inliers).run() wtc_values = gtsam.TranslationRecovery().run(w_iZj_inliers)
wTc_values = gtsam.Values() wTc_values = gtsam.Values()
for key in wRc_values.keys(): for key in wRc_values.keys():

10
python/gtsam/tests/test_TranslationRecovery.py
View File

@ -34,8 +34,10 @@ class TestTranslationRecovery(unittest.TestCase):
def test_constructor(self): def test_constructor(self):
"""Construct from binary measurements.""" """Construct from binary measurements."""
algorithm = gtsam.TranslationRecovery(gtsam.BinaryMeasurementsUnit3()) algorithm = gtsam.TranslationRecovery()
self.assertIsInstance(algorithm, gtsam.TranslationRecovery) self.assertIsInstance(algorithm, gtsam.TranslationRecovery)
algorithm_params = gtsam.TranslationRecovery(gtsam.TranslationRecoveryParams())
self.assertIsInstance(algorithm_params, gtsam.TranslationRecovery)
def test_run(self): def test_run(self):
gt_poses = ExampleValues() gt_poses = ExampleValues()
@ -44,10 +46,12 @@ class TestTranslationRecovery(unittest.TestCase):
# Set verbosity to Silent for tests # Set verbosity to Silent for tests
lmParams = gtsam.LevenbergMarquardtParams() lmParams = gtsam.LevenbergMarquardtParams()
lmParams.setVerbosityLM("silent") lmParams.setVerbosityLM("silent")
params = gtsam.TranslationRecoveryParams()
params.lmParams = lmParams
algorithm = gtsam.TranslationRecovery(measurements, lmParams) algorithm = gtsam.TranslationRecovery(params)
scale = 2.0 scale = 2.0
result = algorithm.run(scale) result = algorithm.run(measurements, scale)
w_aTc = gt_poses.atPose3(2).translation() - gt_poses.atPose3(0).translation() w_aTc = gt_poses.atPose3(2).translation() - gt_poses.atPose3(0).translation()
w_aTb = gt_poses.atPose3(1).translation() - gt_poses.atPose3(0).translation() w_aTb = gt_poses.atPose3(1).translation() - gt_poses.atPose3(0).translation()