Merge pull request #619 from borglab/fix/zero_translation_avg
Handling edges with pure rotation in translation averagingrelease/4.3a0
Akshay Krishnan
GitHub
commit
d6f7da73c3
|
|
@ -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>
|
||||
|
|
@ -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) {
|
||||
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) {
|
||||
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;
|
||||
|
||||
|
|
@ -44,13 +77,14 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const {
|
|||
return graph;
|
||||
}
|
||||
|
||||
void TranslationRecovery::addPrior(const double scale,
|
||||
NonlinearFactorGraph *graph,
|
||||
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 {
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -16,14 +16,16 @@
|
|||
* @brief Recovering translations in an epipolar graph when rotations are given.
|
||||
*/
|
||||
|
||||
#include <map>
|
||||
#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
|
||||
|
|
@ -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_;
|
||||
|
||||
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.
|
||||
*/
|
||||
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.
|
||||
|
|
|
|||
|
|
@ -11,19 +11,29 @@
|
|||
|
||||
/**
|
||||
* @file testTranslationRecovery.cpp
|
||||
* @author Frank Dellaert
|
||||
* @author Frank Dellaert, Akshay Krishnan
|
||||
* @date March 2020
|
||||
* @brief test recovering translations when rotations are given.
|
||||
*/
|
||||
|
||||
#include <gtsam/sfm/TranslationRecovery.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/sfm/TranslationRecovery.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// Returns the Unit3 direction as measured in the binary measurement, but
|
||||
// computed from the input poses. Helper function used in the unit tests.
|
||||
Unit3 GetDirectionFromPoses(const Values& poses,
|
||||
const BinaryMeasurement<Unit3>& unitTranslation) {
|
||||
const Pose3 wTa = poses.at<Pose3>(unitTranslation.key1()),
|
||||
wTb = poses.at<Pose3>(unitTranslation.key2());
|
||||
const Point3 Ta = wTa.translation(), Tb = wTb.translation();
|
||||
return Unit3(Tb - Ta);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// We read the BAL file, which has 3 cameras in it, with poses. We then assume
|
||||
// the rotations are correct, but translations have to be estimated from
|
||||
|
|
@ -48,43 +58,186 @@ TEST(TranslationRecovery, BAL) {
|
|||
const auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
|
||||
poses, {{0, 1}, {0, 2}, {1, 2}});
|
||||
|
||||
// Check
|
||||
Unit3 w_aZb_stored; // measurement between 0 and 1 stored for next unit test
|
||||
for(auto& unitTranslation : relativeTranslations) {
|
||||
const Pose3 wTa = poses.at<Pose3>(unitTranslation.key1()),
|
||||
wTb = poses.at<Pose3>(unitTranslation.key2());
|
||||
const Point3 Ta = wTa.translation(), Tb = wTb.translation();
|
||||
const Unit3 w_aZb = unitTranslation.measured();
|
||||
EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb));
|
||||
if(unitTranslation.key1() == 0 && unitTranslation.key2() == 1) {
|
||||
w_aZb_stored = unitTranslation.measured();
|
||||
}
|
||||
// Check simulated measurements.
|
||||
for (auto& unitTranslation : relativeTranslations) {
|
||||
EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
|
||||
unitTranslation.measured()));
|
||||
}
|
||||
|
||||
TranslationRecovery algorithm(relativeTranslations);
|
||||
const auto graph = algorithm.buildGraph();
|
||||
EXPECT_LONGS_EQUAL(3, graph.size());
|
||||
|
||||
// Translation recovery, version 1
|
||||
// Run translation recovery
|
||||
const double scale = 2.0;
|
||||
const auto result = algorithm.run(scale);
|
||||
|
||||
// Check result for first two translations, determined by prior
|
||||
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
|
||||
EXPECT(assert_equal(Point3(2 * w_aZb_stored.point3()), result.at<Point3>(1)));
|
||||
EXPECT(assert_equal(
|
||||
Point3(2 * GetDirectionFromPoses(poses, relativeTranslations[0])),
|
||||
result.at<Point3>(1)));
|
||||
|
||||
// Check that the third translations is correct
|
||||
Point3 Ta = poses.at<Pose3>(0).translation();
|
||||
Point3 Tb = poses.at<Pose3>(1).translation();
|
||||
Point3 Tc = poses.at<Pose3>(2).translation();
|
||||
Point3 expected =
|
||||
(Tc - Ta) * (scale / (Tb - Ta).norm());
|
||||
Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm());
|
||||
EXPECT(assert_equal(expected, result.at<Point3>(2), 1e-4));
|
||||
|
||||
// TODO(frank): how to get stats back?
|
||||
// EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5);
|
||||
}
|
||||
|
||||
TEST(TranslationRecovery, TwoPoseTest) {
|
||||
// Create a dataset with 2 poses.
|
||||
// __ __
|
||||
// \/ \/
|
||||
// 0 _____ 1
|
||||
//
|
||||
// 0 and 1 face in the same direction but have a translation offset.
|
||||
Values poses;
|
||||
poses.insert<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
|
||||
poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
|
||||
|
||||
auto relativeTranslations =
|
||||
TranslationRecovery::SimulateMeasurements(poses, {{0, 1}});
|
||||
|
||||
// Check simulated measurements.
|
||||
for (auto& unitTranslation : relativeTranslations) {
|
||||
EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
|
||||
unitTranslation.measured()));
|
||||
}
|
||||
|
||||
TranslationRecovery algorithm(relativeTranslations);
|
||||
const auto graph = algorithm.buildGraph();
|
||||
EXPECT_LONGS_EQUAL(1, graph.size());
|
||||
|
||||
// Run translation recovery
|
||||
const auto result = algorithm.run(/*scale=*/3.0);
|
||||
|
||||
// Check result for first two translations, determined by prior
|
||||
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
|
||||
EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1)));
|
||||
}
|
||||
|
||||
TEST(TranslationRecovery, ThreePoseTest) {
|
||||
// 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}, {1, 3}, {3, 0}});
|
||||
|
||||
// Check simulated measurements.
|
||||
for (auto& unitTranslation : relativeTranslations) {
|
||||
EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
|
||||
unitTranslation.measured()));
|
||||
}
|
||||
|
||||
TranslationRecovery algorithm(relativeTranslations);
|
||||
const auto graph = algorithm.buildGraph();
|
||||
EXPECT_LONGS_EQUAL(3, graph.size());
|
||||
|
||||
const auto result = algorithm.run(/*scale=*/3.0);
|
||||
|
||||
// Check result
|
||||
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
|
||||
EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1)));
|
||||
EXPECT(assert_equal(Point3(1.5, -1.5, 0), result.at<Point3>(3)));
|
||||
}
|
||||
|
||||
TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) {
|
||||
// Create a dataset with 3 poses.
|
||||
// __ __
|
||||
// \/ \/
|
||||
// 0 _____ 1
|
||||
// 2 <|
|
||||
//
|
||||
// 0 and 1 face in the same direction but have a translation offset. 2 is at
|
||||
// the same point as 1 but is rotated, with little FOV overlap.
|
||||
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>(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0)));
|
||||
|
||||
auto relativeTranslations =
|
||||
TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}});
|
||||
|
||||
// Check simulated measurements.
|
||||
for (auto& unitTranslation : relativeTranslations) {
|
||||
EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
|
||||
unitTranslation.measured()));
|
||||
}
|
||||
|
||||
TranslationRecovery algorithm(relativeTranslations);
|
||||
const auto graph = algorithm.buildGraph();
|
||||
// There is only 1 non-zero translation edge.
|
||||
EXPECT_LONGS_EQUAL(1, graph.size());
|
||||
|
||||
// Run translation recovery
|
||||
const auto result = algorithm.run(/*scale=*/3.0);
|
||||
|
||||
// Check result
|
||||
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
|
||||
EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1)));
|
||||
EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(2)));
|
||||
}
|
||||
|
||||
TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) {
|
||||
// Create a dataset with 4 poses.
|
||||
// __ __
|
||||
// \/ \/
|
||||
// 0 _____ 1
|
||||
// \ __ 2 <|
|
||||
// \\//
|
||||
// 3
|
||||
//
|
||||
// 0 and 1 face in the same direction but have a translation offset. 2 is at
|
||||
// the same point as 1 but is rotated, with very little FOV overlap. 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>(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0)));
|
||||
poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
|
||||
|
||||
auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
|
||||
poses, {{0, 1}, {1, 2}, {1, 3}, {3, 0}});
|
||||
|
||||
// Check simulated measurements.
|
||||
for (auto& unitTranslation : relativeTranslations) {
|
||||
EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
|
||||
unitTranslation.measured()));
|
||||
}
|
||||
|
||||
TranslationRecovery algorithm(relativeTranslations);
|
||||
const auto graph = algorithm.buildGraph();
|
||||
EXPECT_LONGS_EQUAL(3, graph.size());
|
||||
|
||||
// Run translation recovery
|
||||
const auto result = algorithm.run(/*scale=*/4.0);
|
||||
|
||||
// Check result
|
||||
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
|
||||
EXPECT(assert_equal(Point3(4, 0, 0), result.at<Point3>(1)));
|
||||
EXPECT(assert_equal(Point3(4, 0, 0), result.at<Point3>(2)));
|
||||
EXPECT(assert_equal(Point3(2, -2, 0), result.at<Point3>(3)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue