Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Handling edges with pure rotation in translation averaging #619

Merged
merged 4 commits into from
Dec 2, 2020
Merged

Handling edges with pure rotation in translation averaging #619

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
4bc250e
new test doesnt pass
akshay-krishnan Nov 29, 2020
8d009c2
translation recovery unit tests pass
akshay-krishnan Nov 30, 2020
602db46
changing test names and adding documentation
akshay-krishnan Dec 1, 2020
6d2e306
documenting member variables
akshay-krishnan Dec 1, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
65 changes: 56 additions & 9 deletions gtsam/sfm/TranslationRecovery.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,13 +11,12 @@

/**
* @file TranslationRecovery.cpp
* @author Frank Dellaert
* @author Frank Dellaert, Akshay Krishnan
* @date March 2020
* @brief Source code for recovering translations when rotations are given
*/

#include <gtsam/sfm/TranslationRecovery.h>

#include <gtsam/base/DSFMap.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
Expand All @@ -27,11 +26,45 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/TranslationFactor.h>
#include <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/slam/PriorFactor.h>

#include <set>
#include <utility>

using namespace gtsam;
using namespace std;

TranslationRecovery::TranslationRecovery(
const TranslationRecovery::TranslationEdges &relativeTranslations,
const LevenbergMarquardtParams &lmParams)
: params_(lmParams) {
// Some relative translations may be zero. We treat nodes that have a zero
// relativeTranslation as a single node.

// A DSFMap is used to find sets of nodes that have a zero relative
// translation. Add the nodes in each edge to the DSFMap, and merge nodes that
// are connected by a zero relative translation.
DSFMap<Key> sameTranslationDSF;
for (const auto &edge : relativeTranslations) {
dellaert marked this conversation as resolved.
Show resolved Hide resolved
Key key1 = sameTranslationDSF.find(edge.key1());
Key key2 = sameTranslationDSF.find(edge.key2());
if (key1 != key2 && edge.measured().equals(Unit3(0.0, 0.0, 0.0))) {
sameTranslationDSF.merge(key1, key2);
}
}
// Use only those edges for which two keys have a distinct root in the DSFMap.
for (const auto &edge : relativeTranslations) {
dellaert marked this conversation as resolved.
Show resolved Hide resolved
Key key1 = sameTranslationDSF.find(edge.key1());
Key key2 = sameTranslationDSF.find(edge.key2());
if (key1 == key2) continue;
relativeTranslations_.emplace_back(key1, key2, edge.measured(),
edge.noiseModel());
}
// Store the DSF map for post-processing results.
sameTranslationNodes_ = sameTranslationDSF.sets();
}

NonlinearFactorGraph TranslationRecovery::buildGraph() const {
NonlinearFactorGraph graph;

Expand All @@ -44,13 +77,14 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const {
return graph;
}

void TranslationRecovery::addPrior(const double scale,
NonlinearFactorGraph *graph,
const SharedNoiseModel &priorNoiseModel) const {
void TranslationRecovery::addPrior(
const double scale, 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(),
edge->noiseModel());
graph->emplace_shared<PriorFactor<Point3> >(edge->key1(), Point3(0, 0, 0),
priorNoiseModel);
graph->emplace_shared<PriorFactor<Point3> >(
edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
}

Values TranslationRecovery::initalizeRandomly() const {
Expand All @@ -77,6 +111,19 @@ Values TranslationRecovery::run(const double scale) const {
const Values initial = initalizeRandomly();
LevenbergMarquardtOptimizer lm(graph, initial, params_);
Values result = lm.optimize();

// Nodes that were not optimized are stored in sameTranslationNodes_ as a map
// from a key that was optimized to keys that were not optimized. Iterate over
// map and add results for keys not optimized.
for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) {
Key optimizedKey = optimizedAndDuplicateKeys.first;
std::set<Key> duplicateKeys = optimizedAndDuplicateKeys.second;
// Add the result for the duplicate key if it does not already exist.
for (const Key duplicateKey : duplicateKeys) {
if (result.exists(duplicateKey)) continue;
result.insert<Point3>(duplicateKey, result.at<Point3>(optimizedKey));
}
}
return result;
}

Expand Down
31 changes: 20 additions & 11 deletions gtsam/sfm/TranslationRecovery.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,14 +16,16 @@
* @brief Recovering translations in an epipolar graph when rotations are given.
*/

#include <map>
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Include only what you really need in the header, include others in .cpp only

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

std::map is used in line 59 below, for sameTranslationNodes_.

#include <set>
#include <utility>
#include <vector>

#include <gtsam/geometry/Unit3.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/BinaryMeasurement.h>

#include <utility>
#include <vector>

namespace gtsam {

// Set up an optimization problem for the unknown translations Ti in the world
Expand Down Expand Up @@ -52,23 +54,30 @@ class TranslationRecovery {
using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;

private:
// Translation directions between camera pairs.
TranslationEdges relativeTranslations_;

// Parameters used by the LM Optimizer.
LevenbergMarquardtParams params_;

// Map from a key in the graph to a set of keys that share the same
// translation.
std::map<Key, std::set<Key>> sameTranslationNodes_;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

document all of these with at least a ///< one-liner after declaration


public:
/**
* @brief Construct a new Translation Recovery object
*
* @param relativeTranslations the relative translations, in world coordinate
* frames, vector of BinaryMeasurements of Unit3, where each key of a measurement
* is a point in 3D.
* frames, vector of BinaryMeasurements of Unit3, where each key of a
* measurement is a point in 3D.
* @param lmParams (optional) gtsam::LavenbergMarquardtParams that can be
* used to modify the parameters for the LM optimizer. By default, uses the
* default LM parameters.
* default LM parameters.
*/
TranslationRecovery(const TranslationEdges &relativeTranslations,
const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams())
: relativeTranslations_(relativeTranslations), params_(lmParams) {}
TranslationRecovery(
const TranslationEdges &relativeTranslations,
const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams());

/**
* @brief Build the factor graph to do the optimization.
Expand Down Expand Up @@ -108,8 +117,8 @@ class TranslationRecovery {
*
* @param poses SE(3) ground truth poses stored as Values
* @param edges pairs (a,b) for which a measurement w_aZb will be generated.
* @return TranslationEdges vector of binary measurements where the keys are
* the cameras and the measurement is the simulated Unit3 translation
* @return TranslationEdges vector of binary measurements where the keys are
* the cameras and the measurement is the simulated Unit3 translation
* direction between the cameras.
*/
static TranslationEdges SimulateMeasurements(
Expand Down
Loading