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

Merge pull request #1120 from borglab/ta-seed 

Using a random number generator in translation averaging
release/4.3a0
Akshay Krishnan 2022-03-01 22:03:16 -08:00 committed by GitHub
parent a0206e210d 9f855f44f4
commit a0d64a9448
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 42 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/sfm/ShonanAveraging.h
View File

@ -300,6 +300,7 @@ class GTSAM_EXPORT ShonanAveraging {
/**
* Create initial Values of type SO(p)
* @param p the dimensionality of the rotation manifold
* @param rng random number generator
*/
Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const;

17
gtsam/sfm/TranslationRecovery.cpp
View File

@ -35,6 +35,9 @@
using namespace gtsam;
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)
@ -88,13 +91,15 @@ void TranslationRecovery::addPrior(
edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
}
Values TranslationRecovery::initalizeRandomly() const {
Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) 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 = [&initial](Key j) {
auto insert = [&](Key j) {
if (!initial.exists(j)) {
initial.insert<Point3>(j, Vector3::Random());
initial.insert<Point3>(
j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng)));
}
};
@ -115,10 +120,14 @@ Values TranslationRecovery::initalizeRandomly() const {
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 = initalizeRandomly();
const Values initial = initializeRandomly();
LevenbergMarquardtOptimizer lm(graph, initial, params_);
Values result = lm.optimize();

20
gtsam/sfm/TranslationRecovery.h
View File

@ -16,18 +16,16 @@
* @brief Recovering translations in an epipolar graph when rotations are given.
*/
#pragma once
#include <gtsam/geometry/Unit3.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/BinaryMeasurement.h>
#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>
namespace gtsam {
// Set up an optimization problem for the unknown translations Ti in the world
@ -102,9 +100,17 @@ class TranslationRecovery {
/**
* @brief Create random initial translations.
*
* @param rng random number generator
* @return Values
*/
Values initalizeRandomly() const;
Values initializeRandomly(std::mt19937 *rng) const;
/**
* @brief Version of initializeRandomly with a fixed seed.
*
* @return Values
*/
Values initializeRandomly() const;
/**
* @brief Build and optimize factor graph.

30
tests/testTranslationRecovery.cpp
View File

@ -115,8 +115,8 @@ TEST(TranslationRecovery, TwoPoseTest) {
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)));
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1), 1e-8));
}
TEST(TranslationRecovery, ThreePoseTest) {
@ -152,9 +152,9 @@ TEST(TranslationRecovery, ThreePoseTest) {
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)));
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1), 1e-8));
EXPECT(assert_equal(Point3(1.5, -1.5, 0), result.at<Point3>(3), 1e-8));
}
TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) {
@ -189,9 +189,9 @@ TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) {
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)));
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1), 1e-8));
EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(2), 1e-8));
}
TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) {
@ -230,10 +230,10 @@ TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) {
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)));
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
EXPECT(assert_equal(Point3(4, 0, 0), result.at<Point3>(1), 1e-8));
EXPECT(assert_equal(Point3(4, 0, 0), result.at<Point3>(2), 1e-8));
EXPECT(assert_equal(Point3(2, -2, 0), result.at<Point3>(3), 1e-8));
}
TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
@ -260,9 +260,9 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
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(0, 0, 0), result.at<Point3>(1)));
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(2)));
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(1), 1e-8));
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(2), 1e-8));
}
/* ************************************************************************* */