Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Using a random number generator in translation averaging #1120

Merged
merged 10 commits into from
Mar 2, 2022
Merged

Using a random number generator in translation averaging #1120

Show file tree
Hide file tree
Changes from 9 commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
18b4036
Finish 4.2a1
dellaert Jan 5, 2022
d6f3468
Merge pull request #1024 from borglab/release/4.2a2
dellaert Jan 10, 2022
76c7419
Merge pull request #1041 from borglab/release/4.2a3
dellaert Jan 17, 2022
c10f195
use rng in TA initialization
akshay-krishnan Feb 23, 2022
24173ab
update comment in shonan
akshay-krishnan Feb 23, 2022
93519e4
updating tolerance in tests
akshay-krishnan Feb 25, 2022
1a2eb9a
change to pointer
akshay-krishnan Mar 1, 2022
42bee1a
lamda capture all variables
akshay-krishnan Mar 1, 2022
97ee126
update pointer usage
akshay-krishnan Mar 1, 2022
9f855f4
Merge branch 'develop' into ta-seed
akshay-krishnan Mar 1, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions gtsam/sfm/ShonanAveraging.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
17 changes: 13 additions & 4 deletions gtsam/sfm/TranslationRecovery.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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)));
}
};

Expand All @@ -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();

Expand Down
20 changes: 14 additions & 6 deletions gtsam/sfm/TranslationRecovery.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,16 +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 <map>
#include <set>
#include <utility>
#include <vector>

namespace gtsam {

// Set up an optimization problem for the unknown translations Ti in the world
Expand Down Expand Up @@ -100,9 +100,17 @@ class TranslationRecovery {
/**
* @brief Create random initial translations.
*
* @param rng random number generator
* @return Values
*/
Values initializeRandomly(std::mt19937 *rng) const;

/**
* @brief Version of initializeRandomly with a fixed seed.
*
* @return Values
*/
Values initalizeRandomly() const;
Values initializeRandomly() const;

/**
* @brief Build and optimize factor graph.
Expand Down
30 changes: 15 additions & 15 deletions tests/testTranslationRecovery.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -116,8 +116,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) {
Expand Down Expand Up @@ -153,9 +153,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) {
Expand Down Expand Up @@ -190,9 +190,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) {
Expand Down Expand Up @@ -231,10 +231,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) {
Expand All @@ -261,9 +261,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));
}

/* ************************************************************************* */
Expand Down