Adding options to translation averaging to support betweenfactor translation priors #1190
Conversation
Finish release 4.2a6
|
Cool. But what's this? |
|
Two suggestions before I review:
|
|
I created a branch called ta-add-methods, but it looks like there was one already, and I started working on an existing branch. Pulled from develop now. |
gtsam/sfm/TranslationRecovery.cpp
Outdated
| return graph; | ||
| } | ||
|
|
||
| void TranslationRecovery::addPrior( | ||
| const double scale, NonlinearFactorGraph *graph, | ||
| const double scale, const boost::shared_ptr<NonlinearFactorGraph> graph, |
There was a problem hiding this comment.
I think * is right. In .i file use @ to indicated an output parameter.
There was a problem hiding this comment.
changed all shared_ptr back to *, this change was to wrap these methods, but later I decided not to wrap them.
gtsam/sfm/TranslationRecovery.h
Outdated
| @@ -93,7 +125,8 @@ class TranslationRecovery { | |||
| * @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, | |||
| void addPrior(const double scale, | |||
gtsam/sfm/TranslationRecovery.h
Outdated
| * @return Key of optimized variable - same as input if it does not have any | ||
| * zero-translation edges. | ||
| */ | ||
| Key getUniqueKey(const Key i) const; |
There was a problem hiding this comment.
I have no idea what this does. we can chat about it.
There was a problem hiding this comment.
If there are multiple input nodes that share the same translation, i.e have a zero unit translation edge between them, only one of them will be added to the factor graph. This function will return the value of the Key that was added to the graph for an input node. If there is no zero-translation edge, the output is the same as input.
It is used each time we add the betweenFactors or any other priors, we need to find that "root" node that was added to the graph. I renamed the method, hopefully its more clear.
gtsam/sfm/sfm.i
Outdated
| gtsam::Values run(const gtsam::BinaryMeasurementsPoint3& betweenTranslations, | ||
| const double scale) const; | ||
| // default empty betweenTranslations | ||
| // gtsam::Values run(const double scale) const; |
There was a problem hiding this comment.
@dellaert is there a way to wrap the method this way? using the default value of the first betweenTranslations argument?
There was a problem hiding this comment.
Actually, I think run should have both relativeTranslations and betweenTranslations. The constructor should just take parameters, not data, so you can "run" multiple datasets.
If you agree, you could make scale the first argument and provide two declarations in the .i file:
gtsam::Values run(const double scale, const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const;
gtsam::Values run(const double scale, const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const gtsam::BinaryMeasurementsPoint3& betweenTranslations) const;
|
That was not as simple as it should have been, due to the handling of nodes with zero-edges between them. But its done now, moved the relativeTranslations to |
|
|
||
| // Add global frame prior and scale (either from betweenTranslations or | ||
| // scale). | ||
| addPrior(nonzeroRelativeTranslations, scale, nonzeroBetweenTranslations, |
There was a problem hiding this comment.
Hmmm, you add even if betweenTranslations is not empty
There was a problem hiding this comment.
we do not add the scale prior, only a prior on the first translation to lie at (0, 0, 0). I updated the addPrior documention to be more clear about this.
| return initializeRandomly(relativeTranslations, &kRandomNumberGenerator); | ||
| } | ||
|
|
||
| Values TranslationRecovery::run( |
There was a problem hiding this comment.
I think we can still fine-tune this API, but let's go with this for now.
These Point3 "between translations" (not using the term "relative translations" since this was already used for the direction measurements), can come for other sensors in a SLAM problem.