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

new test doesnt pass

release/4.3a0
akrishnan86 2020-11-28 23:44:20 -08:00
parent c957478da1
commit 4bc250e7c0
3 changed files with 141 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
gtsam/sfm/TranslationRecovery.cpp
View File

@ -16,8 +16,7 @@
* @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,39 @@
#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) {
TranslationEdges tempRelativeTranslations;
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);
}
}
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());
}
sameTranslationNodes_ = sameTranslationDSF.sets();
}
NonlinearFactorGraph TranslationRecovery::buildGraph() const {
NonlinearFactorGraph graph;
@ -44,13 +71,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 {
@ -77,6 +105,15 @@ Values TranslationRecovery::run(const double scale) const {
const Values initial = initalizeRandomly();
LevenbergMarquardtOptimizer lm(graph, initial, params_);
Values result = lm.optimize();
for (const auto &sameTranslationKeys : sameTranslationNodes_) {
Key optimizedKey = sameTranslationKeys.first;
std::set<Key> duplicateKeys = sameTranslationKeys.second;
for (const Key duplicateKey : duplicateKeys) {
if (result.exists(duplicateKey)) continue;
result.insert<Point3>(duplicateKey, result.at<Point3>(optimizedKey));
}
}
return result;
}

6
gtsam/sfm/TranslationRecovery.h
View File

@ -23,6 +23,8 @@
#include <utility>
#include <vector>
#include <set>
#include <map>
namespace gtsam {
@ -54,6 +56,7 @@ class TranslationRecovery {
private:
TranslationEdges relativeTranslations_;
LevenbergMarquardtParams params_;
std::map<Key, std::set<Key>> sameTranslationNodes_;
public:
/**
@ -67,8 +70,7 @@ class TranslationRecovery {
* default LM parameters.
*/
TranslationRecovery(const TranslationEdges &relativeTranslations,
const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams())
: relativeTranslations_(relativeTranslations), params_(lmParams) {}
const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams());
/**
* @brief Build the factor graph to do the optimization.

100
tests/testTranslationRecovery.cpp
View File

@ -16,9 +16,8 @@
* @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;
@ -49,14 +48,14 @@ TEST(TranslationRecovery, BAL) {
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()),
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) {
if (unitTranslation.key1() == 0 && unitTranslation.key2() == 1) {
w_aZb_stored = unitTranslation.measured();
}
}
@ -77,14 +76,99 @@ TEST(TranslationRecovery, BAL) {
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, ZeroRelativeTranslations) {
// Create a graph with 3 cameras.
// __ __
// \/ \/
// 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 very little FOV overlap.
Values poses;
poses.insert(0, Pose3(Rot3(), Point3()));
poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0)));
poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0)));
auto relativeTranslations =
TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}});
// Check
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));
}
TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph();
EXPECT_LONGS_EQUAL(1, graph.size());
// Translation recovery, version 1
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, 0, 0), result.at<Point3>(1)));
EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(2)));
}
TEST(TranslationRecovery, ZeroRelativeTranslations4Cameras) {
// Create a graph with 4 cameras.
// __ __
// \/ \/
// 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(0, Pose3(Rot3(), Point3()));
poses.insert(1, Pose3(Rot3(), Point3(2, 0, 0)));
poses.insert(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0)));
poses.insert(3, Pose3(Rot3(), Point3(1, -1, 0)));
auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
poses, {{0, 1}, {1, 2}, {1, 3}, {3, 0}});
// Check
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));
}
TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph();
EXPECT_LONGS_EQUAL(3, graph.size());
// Translation recovery, version 1
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, 0, 0), result.at<Point3>(1)));
EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(2)));
EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3)));
}
/* ************************************************************************* */
int main() {
TestResult tr;