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

Merge pull request #527 from borglab/fix/translation-recovery 

TranslationRecovery Fixes
release/4.3a0
Varun Agrawal 2020-09-18 11:07:41 -04:00 committed by GitHub
parent 0043120a8a aa67e3c276
commit 076ef20ee6
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 13 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/sfm/TranslationRecovery.cpp
View File

@ -45,9 +45,8 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const {
}
void TranslationRecovery::addPrior(const double scale,
NonlinearFactorGraph *graph) const {
//TODO(akshay-krishnan): make this an input argument
auto priorNoiseModel = noiseModel::Isotropic::Sigma(3, 0.01);
NonlinearFactorGraph *graph,
const SharedNoiseModel &priorNoiseModel) const {
auto edge = relativeTranslations_.begin();
graph->emplace_shared<PriorFactor<Point3> >(edge->key1(), Point3(0, 0, 0), priorNoiseModel);
graph->emplace_shared<PriorFactor<Point3> >(edge->key2(), scale * edge->measured().point3(),

9
gtsam/sfm/TranslationRecovery.h
View File

@ -68,9 +68,7 @@ class TranslationRecovery {
*/
TranslationRecovery(const TranslationEdges &relativeTranslations,
const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams())
: relativeTranslations_(relativeTranslations), params_(lmParams) {
params_.setVerbosityLM("Summary");
}
: relativeTranslations_(relativeTranslations), params_(lmParams) {}
/**
* @brief Build the factor graph to do the optimization.
@ -84,8 +82,11 @@ class TranslationRecovery {
*
* @param scale scale for first relative translation which fixes gauge.
* @param graph factor graph to which prior is added.
* @param priorNoiseModel the noise model to use with the prior.
*/
void addPrior(const double scale, NonlinearFactorGraph *graph) const;
void addPrior(const double scale, NonlinearFactorGraph *graph,
const SharedNoiseModel &priorNoiseModel =
noiseModel::Isotropic::Sigma(3, 0.01)) const;
/**
* @brief Create random initial translations.

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

@ -40,7 +40,12 @@ class TestTranslationRecovery(unittest.TestCase):
def test_run(self):
gt_poses = ExampleValues()
measurements = SimulateMeasurements(gt_poses, [[0, 1], [0, 2], [1, 2]])
algorithm = gtsam.TranslationRecovery(measurements)
# Set verbosity to Silent for tests
lmParams = gtsam.LevenbergMarquardtParams()
lmParams.setVerbosityLM("silent")
algorithm = gtsam.TranslationRecovery(measurements, lmParams)
scale = 2.0
result = algorithm.run(scale)