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

Merge pull request #1190 from borglab/ta-add-methods

release/4.3a0
Frank Dellaert 2022-05-10 21:22:06 -04:00 committed by GitHub
parent fb1da007f8 29787373a6
commit e362906188
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 309 additions and 124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

182
gtsam/sfm/TranslationRecovery.cpp
View File

@ -21,13 +21,16 @@
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h> #include <gtsam/geometry/Unit3.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/TranslationFactor.h> #include <gtsam/sfm/TranslationFactor.h>
#include <gtsam/sfm/TranslationRecovery.h> #include <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/expressions.h>
#include <set> #include <set>
#include <utility> #include <utility>
@ -38,16 +41,13 @@ using namespace std;
// In Wrappers we have no access to this so have a default ready. // In Wrappers we have no access to this so have a default ready.
static std::mt19937 kRandomNumberGenerator(42); static std::mt19937 kRandomNumberGenerator(42);
TranslationRecovery::TranslationRecovery( // Some relative translations may be zero. We treat nodes that have a zero
const TranslationRecovery::TranslationEdges &relativeTranslations, // relativeTranslation as a single node.
const LevenbergMarquardtParams &lmParams) // A DSFMap is used to find sets of nodes that have a zero relative
: params_(lmParams) { // translation. Add the nodes in each edge to the DSFMap, and merge nodes that
// Some relative translations may be zero. We treat nodes that have a zero // are connected by a zero relative translation.
// relativeTranslation as a single node. DSFMap<Key> getSameTranslationDSFMap(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) {
// 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; DSFMap<Key> sameTranslationDSF;
for (const auto &edge : relativeTranslations) { for (const auto &edge : relativeTranslations) {
Key key1 = sameTranslationDSF.find(edge.key1()); Key key1 = sameTranslationDSF.find(edge.key1());
@ -56,94 +56,152 @@ TranslationRecovery::TranslationRecovery(
sameTranslationDSF.merge(key1, key2); sameTranslationDSF.merge(key1, key2);
} }
} }
// Use only those edges for which two keys have a distinct root in the DSFMap. return sameTranslationDSF;
for (const auto &edge : relativeTranslations) {
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 { // 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;
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));
}
}
return final_result;
}
NonlinearFactorGraph TranslationRecovery::buildGraph(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
// Add all relative translation edges // Add translation factors for input translation directions.
for (auto edge : relativeTranslations_) { for (auto edge : relativeTranslations) {
graph.emplace_shared<TranslationFactor>(edge.key1(), edge.key2(), graph.emplace_shared<TranslationFactor>(edge.key1(), edge.key2(),
edge.measured(), edge.noiseModel()); edge.measured(), edge.noiseModel());
} }
return graph; return graph;
} }
void TranslationRecovery::addPrior( 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 { const SharedNoiseModel &priorNoiseModel) const {
auto edge = relativeTranslations_.begin(); auto edge = relativeTranslations.begin();
if (edge == relativeTranslations_.end()) return; if (edge == relativeTranslations.end()) return;
graph->emplace_shared<PriorFactor<Point3> >(edge->key1(), Point3(0, 0, 0), graph->emplace_shared<PriorFactor<Point3>>(edge->key1(), Point3(0, 0, 0),
priorNoiseModel); priorNoiseModel);
graph->emplace_shared<PriorFactor<Point3> >(
edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); // 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); uniform_real_distribution<double> randomVal(-1, 1);
// Create a lambda expression that checks whether value exists and randomly // Create a lambda expression that checks whether value exists and randomly
// initializes if not. // initializes if not.
Values initial; Values initial;
auto insert = [&](Key j) { 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>( initial.insert<Point3>(
j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng))); 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 // Loop over measurements and add a random translation
for (auto edge : relativeTranslations_) { for (auto edge : relativeTranslations) {
insert(edge.key1()); insert(edge.key1());
insert(edge.key2()); insert(edge.key2());
} }
// 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_) {
Key optimizedKey = optimizedAndDuplicateKeys.first;
initial.insert<Point3>(optimizedKey, Point3(0, 0, 0));
}
}
return initial; return initial;
} }
Values TranslationRecovery::initializeRandomly() const { Values TranslationRecovery::initializeRandomly(
return initializeRandomly(&kRandomNumberGenerator); const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const Values &initialValues) const {
return initializeRandomly(relativeTranslations, &kRandomNumberGenerator,
initialValues);
} }
Values TranslationRecovery::run(const double scale) const { Values TranslationRecovery::run(
auto graph = buildGraph(); const TranslationEdges &relativeTranslations, const double scale,
addPrior(scale, &graph); const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
const Values initial = initializeRandomly(); const Values &initialValues) const {
LevenbergMarquardtOptimizer lm(graph, initial, params_); // Find edges that have a zero-translation, and recompute relativeTranslations
Values result = lm.optimize(); // 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);
// Nodes that were not optimized are stored in sameTranslationNodes_ as a map // Create graph of translation factors.
// from a key that was optimized to keys that were not optimized. Iterate over NonlinearFactorGraph graph = buildGraph(nonzeroRelativeTranslations);
// map and add results for keys not optimized.
for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) { // Add global frame prior and scale (either from betweenTranslations or
Key optimizedKey = optimizedAndDuplicateKeys.first; // scale).
std::set<Key> duplicateKeys = optimizedAndDuplicateKeys.second; addPrior(nonzeroRelativeTranslations, scale, nonzeroBetweenTranslations,
// Add the result for the duplicate key if it does not already exist. &graph);
for (const Key duplicateKey : duplicateKeys) {
if (result.exists(duplicateKey)) continue; // Uses initial values from params if provided.
result.insert<Point3>(duplicateKey, result.at<Point3>(optimizedKey)); 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() && !sameTranslationDSFMap.sets().empty()) {
for (const auto &optimizedAndDuplicateKeys : sameTranslationDSFMap.sets()) {
Key optimizedKey = optimizedAndDuplicateKeys.first;
initial.insert<Point3>(optimizedKey, Point3(0, 0, 0));
} }
} }
return result;
LevenbergMarquardtOptimizer lm(graph, initial, lmParams_);
Values result = lm.optimize();
return addSameTranslationNodes(result, sameTranslationDSFMap);
} }
TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements( TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements(

79
gtsam/sfm/TranslationRecovery.h
View File

@ -11,7 +11,7 @@
/** /**
* @file TranslationRecovery.h * @file TranslationRecovery.h
* @author Frank Dellaert * @author Frank Dellaert, Akshay Krishnan
* @date March 2020 * @date March 2020
* @brief Recovering translations in an epipolar graph when rotations are given. * @brief Recovering translations in an epipolar graph when rotations are given.
*/ */
@ -57,68 +57,99 @@ class TranslationRecovery {
// Translation directions between camera pairs. // Translation directions between camera pairs.
TranslationEdges relativeTranslations_; TranslationEdges relativeTranslations_;
// Parameters used by the LM Optimizer. // Parameters.
LevenbergMarquardtParams params_; LevenbergMarquardtParams lmParams_;
// Map from a key in the graph to a set of keys that share the same
// translation.
std::map<Key, std::set<Key>> sameTranslationNodes_;
public: public:
/** /**
* @brief Construct a new Translation Recovery object * @brief Construct a new Translation Recovery object
* *
* @param relativeTranslations the relative translations, in world coordinate * @param lmParams parameters for optimization.
* 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.
*/ */
TranslationRecovery( TranslationRecovery(const LevenbergMarquardtParams &lmParams)
const TranslationEdges &relativeTranslations, : lmParams_(lmParams) {}
const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams());
/**
* @brief Default constructor.
*/
TranslationRecovery() = default;
/** /**
* @brief Build the factor graph to do the optimization. * @brief Build the factor graph to do the optimization.
* *
* @param relativeTranslations unit translation directions between
* translations to be estimated
* @return NonlinearFactorGraph * @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 scale scale for first relative translation which fixes gauge.
* @param graph factor graph to which prior is added. * @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. * @param priorNoiseModel the noise model to use with the prior.
*/ */
void addPrior(const double scale, NonlinearFactorGraph *graph, void addPrior(
const SharedNoiseModel &priorNoiseModel = const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
noiseModel::Isotropic::Sigma(3, 0.01)) const; 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. * @brief Create random initial translations.
* *
* @param relativeTranslations unit translation directions between
* translations to be estimated
* @param rng random number generator * @param rng random number generator
* @param intialValues (optional) initial values from a prior
* @return Values * @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. * @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 * @return Values
*/ */
Values initializeRandomly() const; Values initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const Values &initialValues = Values()) const;
/** /**
* @brief Build and optimize factor graph. * @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. * @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 * @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 * @brief Simulate translation direction measurements

49
gtsam/sfm/sfm.i
View File

@ -4,6 +4,8 @@
namespace gtsam { namespace gtsam {
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/SfmTrack.h> #include <gtsam/sfm/SfmTrack.h>
class SfmTrack { class SfmTrack {
SfmTrack(); SfmTrack();
@ -88,6 +90,7 @@ class BinaryMeasurement {
typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3; typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3; typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
typedef gtsam::BinaryMeasurement<gtsam::Point3> BinaryMeasurementPoint3;
class BinaryMeasurementsUnit3 { class BinaryMeasurementsUnit3 {
BinaryMeasurementsUnit3(); BinaryMeasurementsUnit3();
@ -96,6 +99,13 @@ class BinaryMeasurementsUnit3 {
void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement); void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
}; };
class BinaryMeasurementsPoint3 {
BinaryMeasurementsPoint3();
size_t size() const;
gtsam::BinaryMeasurement<gtsam::Point3> at(size_t idx) const;
void push_back(const gtsam::BinaryMeasurement<gtsam::Point3>& measurement);
};
class BinaryMeasurementsRot3 { class BinaryMeasurementsRot3 {
BinaryMeasurementsRot3(); BinaryMeasurementsRot3();
size_t size() const; size_t size() const;
@ -154,8 +164,8 @@ class ShonanAveraging2 {
ShonanAveraging2(string g2oFile); ShonanAveraging2(string g2oFile);
ShonanAveraging2(string g2oFile, ShonanAveraging2(string g2oFile,
const gtsam::ShonanAveragingParameters2& parameters); const gtsam::ShonanAveragingParameters2& parameters);
ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors, ShonanAveraging2(const gtsam::BetweenFactorPose2s& factors,
const gtsam::ShonanAveragingParameters2 &parameters); const gtsam::ShonanAveragingParameters2& parameters);
// Query properties // Query properties
size_t nrUnknowns() const; size_t nrUnknowns() const;
@ -268,15 +278,36 @@ class MFAS {
}; };
#include <gtsam/sfm/TranslationRecovery.h> #include <gtsam/sfm/TranslationRecovery.h>
class TranslationRecovery { class TranslationRecovery {
TranslationRecovery( TranslationRecovery(const gtsam::LevenbergMarquardtParams& 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 double scale,
const gtsam::BinaryMeasurementsPoint3& betweenTranslations,
const gtsam::Values& initialValues) const;
// default random initial values
gtsam::Values run(
const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
const gtsam::LevenbergMarquardtParams& lmParams); const double scale,
TranslationRecovery( const gtsam::BinaryMeasurementsPoint3& betweenTranslations) const;
const gtsam::BinaryMeasurementsUnit3& // default empty betweenTranslations
relativeTranslations); // default LevenbergMarquardtParams gtsam::Values run(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
gtsam::Values run(const double scale) const; const double scale) const;
gtsam::Values run() const; // default scale = 1.0 // default scale = 1.0, empty betweenTranslations
gtsam::Values run(
const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const;
}; };
} // namespace gtsam } // namespace gtsam

1
gtsam/slam/dataset.h
View File

@ -223,6 +223,7 @@ parse3DFactors(const std::string &filename,
size_t maxIndex = 0); size_t maxIndex = 0);
using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>; using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
using BinaryMeasurementsPoint3 = std::vector<BinaryMeasurement<Point3>>;
using BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>; using BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42

2
python/CMakeLists.txt
View File

@ -47,6 +47,7 @@ set(ignore
gtsam::Pose3Vector gtsam::Pose3Vector
gtsam::Rot3Vector gtsam::Rot3Vector
gtsam::KeyVector gtsam::KeyVector
gtsam::BinaryMeasurementsPoint3
gtsam::BinaryMeasurementsUnit3 gtsam::BinaryMeasurementsUnit3
gtsam::BinaryMeasurementsRot3 gtsam::BinaryMeasurementsRot3
gtsam::DiscreteKey gtsam::DiscreteKey
@ -137,6 +138,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
gtsam::Pose3Vector gtsam::Pose3Vector
gtsam::KeyVector gtsam::KeyVector
gtsam::FixedLagSmootherKeyTimestampMapValue gtsam::FixedLagSmootherKeyTimestampMapValue
gtsam::BinaryMeasurementsPoint3
gtsam::BinaryMeasurementsUnit3 gtsam::BinaryMeasurementsUnit3
gtsam::BinaryMeasurementsRot3 gtsam::BinaryMeasurementsRot3
gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3_S2

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():

2
python/gtsam/specializations/sfm.h
View File

@ -11,6 +11,8 @@
* and saves one copy operation. * and saves one copy operation.
*/ */
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Point3> > >(
m_, "BinaryMeasurementsPoint3");
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >( py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
m_, "BinaryMeasurementsUnit3"); m_, "BinaryMeasurementsUnit3");
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Rot3> > >( py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Rot3> > >(

8
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.LevenbergMarquardtParams())
self.assertIsInstance(algorithm_params, gtsam.TranslationRecovery)
def test_run(self): def test_run(self):
gt_poses = ExampleValues() gt_poses = ExampleValues()
@ -45,9 +47,9 @@ class TestTranslationRecovery(unittest.TestCase):
lmParams = gtsam.LevenbergMarquardtParams() lmParams = gtsam.LevenbergMarquardtParams()
lmParams.setVerbosityLM("silent") lmParams.setVerbosityLM("silent")
algorithm = gtsam.TranslationRecovery(measurements, lmParams) algorithm = gtsam.TranslationRecovery(lmParams)
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()

108
tests/testTranslationRecovery.cpp
View File

@ -17,8 +17,8 @@
*/ */
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/sfm/SfmData.h> #include <gtsam/sfm/SfmData.h>
#include <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/slam/dataset.h> #include <gtsam/slam/dataset.h>
using namespace std; using namespace std;
@ -62,13 +62,13 @@ TEST(TranslationRecovery, BAL) {
unitTranslation.measured())); unitTranslation.measured()));
} }
TranslationRecovery algorithm(relativeTranslations); TranslationRecovery algorithm;
const auto graph = algorithm.buildGraph(); const auto graph = algorithm.buildGraph(relativeTranslations);
EXPECT_LONGS_EQUAL(3, graph.size()); EXPECT_LONGS_EQUAL(3, graph.size());
// Run translation recovery // Run translation recovery
const double scale = 2.0; const double scale = 2.0;
const auto result = algorithm.run(scale); const auto result = algorithm.run(relativeTranslations, scale);
// Check result for first two translations, determined by prior // Check result for first two translations, determined by prior
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0))); EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
@ -107,12 +107,12 @@ TEST(TranslationRecovery, TwoPoseTest) {
unitTranslation.measured())); unitTranslation.measured()));
} }
TranslationRecovery algorithm(relativeTranslations); TranslationRecovery algorithm;
const auto graph = algorithm.buildGraph(); const auto graph = algorithm.buildGraph(relativeTranslations);
EXPECT_LONGS_EQUAL(1, graph.size()); EXPECT_LONGS_EQUAL(1, graph.size());
// Run translation recovery // Run translation recovery
const auto result = algorithm.run(/*scale=*/3.0); const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0);
// Check result for first two translations, determined by prior // Check result for first two translations, determined by prior
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8)); EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@ -145,11 +145,11 @@ TEST(TranslationRecovery, ThreePoseTest) {
unitTranslation.measured())); unitTranslation.measured()));
} }
TranslationRecovery algorithm(relativeTranslations); TranslationRecovery algorithm;
const auto graph = algorithm.buildGraph(); const auto graph = algorithm.buildGraph(relativeTranslations);
EXPECT_LONGS_EQUAL(3, graph.size()); EXPECT_LONGS_EQUAL(3, graph.size());
const auto result = algorithm.run(/*scale=*/3.0); const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0);
// Check result // Check result
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8)); EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@ -180,13 +180,9 @@ TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) {
unitTranslation.measured())); unitTranslation.measured()));
} }
TranslationRecovery algorithm(relativeTranslations); TranslationRecovery algorithm;
const auto graph = algorithm.buildGraph();
// There is only 1 non-zero translation edge.
EXPECT_LONGS_EQUAL(1, graph.size());
// Run translation recovery // Run translation recovery
const auto result = algorithm.run(/*scale=*/3.0); const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0);
// Check result // Check result
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8)); EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@ -222,12 +218,10 @@ TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) {
unitTranslation.measured())); unitTranslation.measured()));
} }
TranslationRecovery algorithm(relativeTranslations); TranslationRecovery algorithm;
const auto graph = algorithm.buildGraph();
EXPECT_LONGS_EQUAL(3, graph.size());
// Run translation recovery // Run translation recovery
const auto result = algorithm.run(/*scale=*/4.0); const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0);
// Check result // Check result
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8)); EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@ -251,13 +245,10 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
unitTranslation.measured())); unitTranslation.measured()));
} }
TranslationRecovery algorithm(relativeTranslations); TranslationRecovery algorithm;
const auto graph = algorithm.buildGraph();
// Graph size will be zero as there no 'non-zero distance' edges.
EXPECT_LONGS_EQUAL(0, graph.size());
// Run translation recovery // Run translation recovery
const auto result = algorithm.run(/*scale=*/4.0); const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0);
// Check result // Check result
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8)); EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@ -265,6 +256,73 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(2), 1e-8)); EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(2), 1e-8));
} }
TEST(TranslationRecovery, ThreePosesWithOneSoftConstraint) {
// Create a dataset with 3 poses.
// __ __
// \/ \/
// 0 _____ 1
// \ __ /
// \\//
// 3
//
// 0 and 1 face in the same direction but have a translation offset. 3 is in
// the same direction as 0 and 1, in between 0 and 1, with some Y axis offset.
Values poses;
poses.insert<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
poses, {{0, 1}, {0, 3}, {1, 3}});
std::vector<BinaryMeasurement<Point3>> betweenTranslations;
betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0),
noiseModel::Isotropic::Sigma(3, 1e-2));
TranslationRecovery algorithm;
auto result =
algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations);
// Check result
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
}
TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) {
// Create a dataset with 3 poses.
// __ __
// \/ \/
// 0 _____ 1
// \ __ /
// \\//
// 3
//
// 0 and 1 face in the same direction but have a translation offset. 3 is in
// the same direction as 0 and 1, in between 0 and 1, with some Y axis offset.
Values poses;
poses.insert<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
poses, {{0, 1}, {0, 3}, {1, 3}});
std::vector<BinaryMeasurement<Point3>> betweenTranslations;
betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0),
noiseModel::Constrained::All(3, 1e2));
TranslationRecovery algorithm;
auto result =
algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations);
// Check result
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;