Added initalizeRandomly
parent
146f411a99
commit
d8f3ca46a4
|
@ -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;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue