Skip to content

Commit

Permalink
Merge pull request #1190 from borglab/ta-add-methods
Browse files Browse the repository at this point in the history
  • Loading branch information
@dellaert
dellaert authored May 11, 2022
2 parents fb1da00 + 2978737 commit e362906
Show file tree
Hide file tree
Showing 9 changed files with 306 additions and 121 deletions.
176 changes: 117 additions & 59 deletions gtsam/sfm/TranslationRecovery.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,13 +21,16 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/TranslationFactor.h>
#include <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/expressions.h>

#include <set>
#include <utility>
Expand All @@ -38,16 +41,13 @@ using namespace std;
// In Wrappers we have no access to this so have a default ready.
static std::mt19937 kRandomNumberGenerator(42);

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.
// 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> getSameTranslationDSFMap(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) {
DSFMap<Key> sameTranslationDSF;
for (const auto &edge : relativeTranslations) {
Key key1 = sameTranslationDSF.find(edge.key1());
Expand All @@ -56,94 +56,152 @@ TranslationRecovery::TranslationRecovery(
sameTranslationDSF.merge(key1, key2);
}
}
// Use only those edges for which two keys have a distinct root in the DSFMap.
for (const auto &edge : relativeTranslations) {
Key key1 = sameTranslationDSF.find(edge.key1());
Key key2 = sameTranslationDSF.find(edge.key2());
return sameTranslationDSF;
}

// Removes zero-translation edges from measurements, and combines the nodes in
// these edges into a single node.
template <typename T>
std::vector<BinaryMeasurement<T>> removeSameTranslationNodes(
const std::vector<BinaryMeasurement<T>> &edges,
const DSFMap<Key> &sameTranslationDSFMap) {
std::vector<BinaryMeasurement<T>> newEdges;
for (const auto &edge : edges) {
Key key1 = sameTranslationDSFMap.find(edge.key1());
Key key2 = sameTranslationDSFMap.find(edge.key2());
if (key1 == key2) continue;
relativeTranslations_.emplace_back(key1, key2, edge.measured(),
edge.noiseModel());
newEdges.emplace_back(key1, key2, edge.measured(), edge.noiseModel());
}
return newEdges;
}

// Adds nodes that were not optimized for because they were connected
// to another node with a zero-translation edge in the input.
Values addSameTranslationNodes(const Values &result,
const DSFMap<Key> &sameTranslationDSFMap) {
Values final_result = result;
// 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 : sameTranslationDSFMap.sets()) {
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 (final_result.exists(duplicateKey)) continue;
final_result.insert<Point3>(duplicateKey,
final_result.at<Point3>(optimizedKey));
}
}
// Store the DSF map for post-processing results.
sameTranslationNodes_ = sameTranslationDSF.sets();
return final_result;
}

NonlinearFactorGraph TranslationRecovery::buildGraph() const {
NonlinearFactorGraph TranslationRecovery::buildGraph(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const {
NonlinearFactorGraph graph;

// Add all relative translation edges
for (auto edge : relativeTranslations_) {
// Add translation factors for input translation directions.
for (auto edge : relativeTranslations) {
graph.emplace_shared<TranslationFactor>(edge.key1(), edge.key2(),
edge.measured(), edge.noiseModel());
}

return graph;
}

void TranslationRecovery::addPrior(
const double scale, NonlinearFactorGraph *graph,
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const double scale,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
NonlinearFactorGraph *graph,
const SharedNoiseModel &priorNoiseModel) const {
auto edge = relativeTranslations_.begin();
if (edge == relativeTranslations_.end()) return;
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());
auto edge = relativeTranslations.begin();
if (edge == relativeTranslations.end()) return;
graph->emplace_shared<PriorFactor<Point3>>(edge->key1(), Point3(0, 0, 0),
priorNoiseModel);

// Add between factors for optional relative translations.
for (auto edge : betweenTranslations) {
graph->emplace_shared<BetweenFactor<Point3>>(
edge.key1(), edge.key2(), edge.measured(), edge.noiseModel());
}

// Add a scale prior only if no other between factors were added.
if (betweenTranslations.empty()) {
graph->emplace_shared<PriorFactor<Point3>>(
edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
}
}

Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const {
Values TranslationRecovery::initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
std::mt19937 *rng, const Values &initialValues) const {
uniform_real_distribution<double> randomVal(-1, 1);
// Create a lambda expression that checks whether value exists and randomly
// initializes if not.
Values initial;
auto insert = [&](Key j) {
if (!initial.exists(j)) {
if (initial.exists(j)) return;
if (initialValues.exists(j)) {
initial.insert<Point3>(j, initialValues.at<Point3>(j));
} else {
initial.insert<Point3>(
j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng)));
}
// Assumes all nodes connected by zero-edges have the same initialization.
};

// Loop over measurements and add a random translation
for (auto edge : relativeTranslations_) {
for (auto edge : relativeTranslations) {
insert(edge.key1());
insert(edge.key2());
}
return initial;
}

Values TranslationRecovery::initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const Values &initialValues) const {
return initializeRandomly(relativeTranslations, &kRandomNumberGenerator,
initialValues);
}

Values TranslationRecovery::run(
const TranslationEdges &relativeTranslations, const double scale,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
const Values &initialValues) const {
// Find edges that have a zero-translation, and recompute relativeTranslations
// and betweenTranslations by retaining only one node for every zero-edge.
DSFMap<Key> sameTranslationDSFMap =
getSameTranslationDSFMap(relativeTranslations);
const TranslationEdges nonzeroRelativeTranslations =
removeSameTranslationNodes(relativeTranslations, sameTranslationDSFMap);
const std::vector<BinaryMeasurement<Point3>> nonzeroBetweenTranslations =
removeSameTranslationNodes(betweenTranslations, sameTranslationDSFMap);

// Create graph of translation factors.
NonlinearFactorGraph graph = buildGraph(nonzeroRelativeTranslations);

// Add global frame prior and scale (either from betweenTranslations or
// scale).
addPrior(nonzeroRelativeTranslations, scale, nonzeroBetweenTranslations,
&graph);

// Uses initial values from params if provided.
Values initial =
initializeRandomly(nonzeroRelativeTranslations, initialValues);

// If there are no valid edges, but zero-distance edges exist, initialize one
// of the nodes in a connected component of zero-distance edges.
if (initial.empty() && !sameTranslationNodes_.empty()) {
for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) {
if (initial.empty() && !sameTranslationDSFMap.sets().empty()) {
for (const auto &optimizedAndDuplicateKeys : sameTranslationDSFMap.sets()) {
Key optimizedKey = optimizedAndDuplicateKeys.first;
initial.insert<Point3>(optimizedKey, Point3(0, 0, 0));
}
}
return initial;
}

Values TranslationRecovery::initializeRandomly() const {
return initializeRandomly(&kRandomNumberGenerator);
}

Values TranslationRecovery::run(const double scale) const {
auto graph = buildGraph();
addPrior(scale, &graph);
const Values initial = initializeRandomly();
LevenbergMarquardtOptimizer lm(graph, initial, params_);
LevenbergMarquardtOptimizer lm(graph, initial, lmParams_);
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;
return addSameTranslationNodes(result, sameTranslationDSFMap);
}

TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements(
Expand Down
79 changes: 55 additions & 24 deletions gtsam/sfm/TranslationRecovery.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@

/**
* @file TranslationRecovery.h
* @author Frank Dellaert
* @author Frank Dellaert, Akshay Krishnan
* @date March 2020
* @brief Recovering translations in an epipolar graph when rotations are given.
*/
Expand Down Expand Up @@ -57,68 +57,99 @@ class TranslationRecovery {
// 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_;
// Parameters.
LevenbergMarquardtParams lmParams_;

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.
* @param lmParams (optional) gtsam::LavenbergMarquardtParams that can be
* used to modify the parameters for the LM optimizer. By default, uses the
* default LM parameters.
* @param lmParams parameters for optimization.
*/
TranslationRecovery(const LevenbergMarquardtParams &lmParams)
: lmParams_(lmParams) {}

/**
* @brief Default constructor.
*/
TranslationRecovery(
const TranslationEdges &relativeTranslations,
const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams());
TranslationRecovery() = default;

/**
* @brief Build the factor graph to do the optimization.
*
* @param relativeTranslations unit translation directions between
* translations to be estimated
* @return NonlinearFactorGraph
*/
NonlinearFactorGraph buildGraph() const;
NonlinearFactorGraph buildGraph(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const;

/**
* @brief Add priors on ednpoints of first measurement edge.
* @brief Add 3 factors to the graph:
* - A prior on the first point to lie at (0, 0, 0)
* - If betweenTranslations is non-empty, between factors provided by it.
* - If betweenTranslations is empty, a prior on scale of the first
* relativeTranslations edge.
*
* @param relativeTranslations unit translation directions between
* translations to be estimated
* @param scale scale for first relative translation which fixes gauge.
* @param graph factor graph to which prior is added.
* @param betweenTranslations relative translations (with scale) between 2
* points in world coordinate frame known a priori.
* @param priorNoiseModel the noise model to use with the prior.
*/
void addPrior(const double scale, NonlinearFactorGraph *graph,
const SharedNoiseModel &priorNoiseModel =
noiseModel::Isotropic::Sigma(3, 0.01)) const;
void addPrior(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const double scale,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
NonlinearFactorGraph *graph,
const SharedNoiseModel &priorNoiseModel =
noiseModel::Isotropic::Sigma(3, 0.01)) const;

/**
* @brief Create random initial translations.
*
* @param relativeTranslations unit translation directions between
* translations to be estimated
* @param rng random number generator
* @param intialValues (optional) initial values from a prior
* @return Values
*/
Values initializeRandomly(std::mt19937 *rng) const;
Values initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
std::mt19937 *rng, const Values &initialValues = Values()) const;

/**
* @brief Version of initializeRandomly with a fixed seed.
*
* @param relativeTranslations unit translation directions between
* translations to be estimated
* @param initialValues (optional) initial values from a prior
* @return Values
*/
Values initializeRandomly() const;
Values initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const Values &initialValues = Values()) const;

/**
* @brief Build and optimize factor graph.
*
* @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.
* @param scale scale for first relative translation which fixes gauge.
* The scale is only used if betweenTranslations is empty.
* @param betweenTranslations relative translations (with scale) between 2
* points in world coordinate frame known a priori.
* @param initialValues intial values for optimization. Initializes randomly
* if not provided.
* @return Values
*/
Values run(const double scale = 1.0) const;
Values run(
const TranslationEdges &relativeTranslations, const double scale = 1.0,
const std::vector<BinaryMeasurement<Point3>> &betweenTranslations = {},
const Values &initialValues = Values()) const;

/**
* @brief Simulate translation direction measurements
Expand Down
Loading

0 comments on commit e362906

Please sign in to comment.