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/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>
@ -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());
@ -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());
if (key1 == key2) continue;
relativeTranslations_.emplace_back(key1, key2, edge.measured(),
edge.noiseModel());
}
// Store the DSF map for post-processing results.
sameTranslationNodes_ = sameTranslationDSF.sets();
return sameTranslationDSF;
}
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;
// 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());
}
// 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;
}
Values TranslationRecovery::initializeRandomly() const {
return initializeRandomly(&kRandomNumberGenerator);
Values TranslationRecovery::initializeRandomly(
const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
const Values &initialValues) const {
return initializeRandomly(relativeTranslations, &kRandomNumberGenerator,
initialValues);
}
Values TranslationRecovery::run(const double scale) const {
auto graph = buildGraph();
addPrior(scale, &graph);
const Values initial = initializeRandomly();
LevenbergMarquardtOptimizer lm(graph, initial, params_);
Values result = lm.optimize();
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);
// 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));
// 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() && !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(

79
gtsam/sfm/TranslationRecovery.h
View File

@ -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.
*/
@ -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 TranslationEdges &relativeTranslations,
const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams());
TranslationRecovery(const LevenbergMarquardtParams &lmParams)
: lmParams_(lmParams) {}
/**
* @brief Default constructor.
*/
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

49
gtsam/sfm/sfm.i
View File

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

1
gtsam/slam/dataset.h
View File

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

2
python/CMakeLists.txt
View File

@ -47,6 +47,7 @@ set(ignore
gtsam::Pose3Vector
gtsam::Rot3Vector
gtsam::KeyVector
gtsam::BinaryMeasurementsPoint3
gtsam::BinaryMeasurementsUnit3
gtsam::BinaryMeasurementsRot3
gtsam::DiscreteKey
@ -137,6 +138,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
gtsam::Pose3Vector
gtsam::KeyVector
gtsam::FixedLagSmootherKeyTimestampMapValue
gtsam::BinaryMeasurementsPoint3
gtsam::BinaryMeasurementsUnit3
gtsam::BinaryMeasurementsRot3
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)
# 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()
for key in wRc_values.keys():

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

@ -11,6 +11,8 @@
* 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> > >(
m_, "BinaryMeasurementsUnit3");
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):
"""Construct from binary measurements."""
algorithm = gtsam.TranslationRecovery(gtsam.BinaryMeasurementsUnit3())
algorithm = gtsam.TranslationRecovery()
self.assertIsInstance(algorithm, gtsam.TranslationRecovery)
algorithm_params = gtsam.TranslationRecovery(gtsam.LevenbergMarquardtParams())
self.assertIsInstance(algorithm_params, gtsam.TranslationRecovery)
def test_run(self):
gt_poses = ExampleValues()
@ -45,9 +47,9 @@ class TestTranslationRecovery(unittest.TestCase):
lmParams = gtsam.LevenbergMarquardtParams()
lmParams.setVerbosityLM("silent")
algorithm = gtsam.TranslationRecovery(measurements, lmParams)
algorithm = gtsam.TranslationRecovery(lmParams)
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_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 <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/sfm/SfmData.h>
#include <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/slam/dataset.h>
using namespace std;
@ -62,13 +62,13 @@ TEST(TranslationRecovery, BAL) {
unitTranslation.measured()));
}
TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph();
TranslationRecovery algorithm;
const auto graph = algorithm.buildGraph(relativeTranslations);
EXPECT_LONGS_EQUAL(3, graph.size());
// Run translation recovery
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
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
@ -107,12 +107,12 @@ TEST(TranslationRecovery, TwoPoseTest) {
unitTranslation.measured()));
}
TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph();
TranslationRecovery algorithm;
const auto graph = algorithm.buildGraph(relativeTranslations);
EXPECT_LONGS_EQUAL(1, graph.size());
// 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
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@ -145,11 +145,11 @@ TEST(TranslationRecovery, ThreePoseTest) {
unitTranslation.measured()));
}
TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph();
TranslationRecovery algorithm;
const auto graph = algorithm.buildGraph(relativeTranslations);
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
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@ -180,13 +180,9 @@ TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) {
unitTranslation.measured()));
}
TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph();
// There is only 1 non-zero translation edge.
EXPECT_LONGS_EQUAL(1, graph.size());
TranslationRecovery algorithm;
// Run translation recovery
const auto result = algorithm.run(/*scale=*/3.0);
const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0);
// Check result
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@ -222,12 +218,10 @@ TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) {
unitTranslation.measured()));
}
TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph();
EXPECT_LONGS_EQUAL(3, graph.size());
TranslationRecovery algorithm;
// Run translation recovery
const auto result = algorithm.run(/*scale=*/4.0);
const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0);
// Check result
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@ -251,13 +245,10 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
unitTranslation.measured()));
}
TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph();
// Graph size will be zero as there no 'non-zero distance' edges.
EXPECT_LONGS_EQUAL(0, graph.size());
TranslationRecovery algorithm;
// Run translation recovery
const auto result = algorithm.run(/*scale=*/4.0);
const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0);
// Check result
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));
}
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() {
TestResult tr;