Added initalizeRandomly

release/4.3a0
Frank Dellaert 2020-03-08 01:58:22 -05:00
parent 146f411a99
commit d8f3ca46a4
1 changed files with 26 additions and 3 deletions

View File

@ -167,6 +167,29 @@ class TranslationRecovery {
return graph;
}
/**
* @brief Create random initial translations.
*
* @return Values
*/
Values initalizeRandomly() const {
Values initial;
auto insert = [&initial](Key j) {
if (!initial.exists(j)) {
initial.insert<Point3>(j, Vector3::Random());
}
};
// Loop over measurements and add a random translation
for (auto edge : relativeTranslations_) {
Key a, b;
std::tie(a, b) = edge.first;
insert(a);
insert(b);
}
return initial;
}
/**
* @brief Build and optimize factor graph.
*
@ -175,11 +198,11 @@ class TranslationRecovery {
*/
Values run(const double scale = 1.0) const {
const auto graph = buildGraph();
// Values initial = randomTranslations();
const Values initial = initalizeRandomly();
// LevenbergMarquardtOptimizer lm(graph, initial);
LevenbergMarquardtOptimizer lm(graph, initial);
Values result; // = lm.optimize();
Values result = lm.optimize();
return result;
}