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

use rng in TA initialization

release/4.3a0
Akshay Krishnan 2022-02-22 22:42:14 -08:00
parent 76c74195de
commit c10f195492
2 changed files with 23 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/sfm/TranslationRecovery.cpp
View File

@ -35,6 +35,9 @@
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;
// In Wrappers we have no access to this so have a default ready.
static std::mt19937 kRandomNumberGenerator(42);
TranslationRecovery::TranslationRecovery( TranslationRecovery::TranslationRecovery(
const TranslationRecovery::TranslationEdges &relativeTranslations, const TranslationRecovery::TranslationEdges &relativeTranslations,
const LevenbergMarquardtParams &lmParams) const LevenbergMarquardtParams &lmParams)
@ -88,13 +91,15 @@ void TranslationRecovery::addPrior(
edge->key2(), scale * edge->measured().point3(), edge->noiseModel()); 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 // Create a lambda expression that checks whether value exists and randomly
// initializes if not. // initializes if not.
Values initial; Values initial;
auto insert = [&initial](Key j) { auto insert = [&initial, &rng, &randomVal](Key j) {
if (!initial.exists(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; return initial;
} }
Values TranslationRecovery::initializeRandomly() const {
return initializeRandomly(kRandomNumberGenerator);
}
Values TranslationRecovery::run(const double scale) const { Values TranslationRecovery::run(const double scale) const {
auto graph = buildGraph(); auto graph = buildGraph();
addPrior(scale, &graph); addPrior(scale, &graph);
const Values initial = initalizeRandomly(); const Values initial = initializeRandomly();
LevenbergMarquardtOptimizer lm(graph, initial, params_); LevenbergMarquardtOptimizer lm(graph, initial, params_);
Values result = lm.optimize(); Values result = lm.optimize();

12
gtsam/sfm/TranslationRecovery.h
View File

@ -99,10 +99,18 @@ class TranslationRecovery {
/** /**
* @brief Create random initial translations. * @brief Create random initial translations.
* *
* @param rng random number generator
* @return Values * @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. * @brief Build and optimize factor graph.