unit test + fix segfault
parent
f5ff7aa49f
commit
a2f99ac71c
|
@ -81,6 +81,7 @@ void TranslationRecovery::addPrior(
|
|||
const double scale, NonlinearFactorGraph *graph,
|
||||
const SharedNoiseModel &priorNoiseModel) const {
|
||||
auto edge = relativeTranslations_.begin();
|
||||
if(edge == relativeTranslations_.end()) return;
|
||||
graph->emplace_shared<PriorFactor<Point3> >(edge->key1(), Point3(0, 0, 0),
|
||||
priorNoiseModel);
|
||||
graph->emplace_shared<PriorFactor<Point3> >(
|
||||
|
@ -102,6 +103,15 @@ Values TranslationRecovery::initalizeRandomly() const {
|
|||
insert(edge.key1());
|
||||
insert(edge.key2());
|
||||
}
|
||||
|
||||
// If there are no valid edges, but zero-distance edges exist, initialize one of
|
||||
// the nodes in a connected component of zero-distance edges.
|
||||
if(initial.empty() && !sameTranslationNodes_.empty()){
|
||||
for(const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) {
|
||||
Key optimizedKey = optimizedAndDuplicateKeys.first;
|
||||
initial.insert<Point3>(optimizedKey, Point3(0, 0, 0));
|
||||
}
|
||||
}
|
||||
return initial;
|
||||
}
|
||||
|
||||
|
|
|
@ -238,6 +238,35 @@ TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) {
|
|||
EXPECT(assert_equal(Point3(2, -2, 0), result.at<Point3>(3)));
|
||||
}
|
||||
|
||||
TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
|
||||
Values poses;
|
||||
poses.insert<Pose3>(0, Pose3(Rot3::RzRyRx(-M_PI / 6, 0, 0), Point3(0, 0, 0)));
|
||||
poses.insert<Pose3>(1, Pose3(Rot3(), Point3(0, 0, 0)));
|
||||
poses.insert<Pose3>(2, Pose3(Rot3::RzRyRx(M_PI / 6, 0, 0), Point3(0, 0, 0)));
|
||||
|
||||
auto relativeTranslations = TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}, {2, 0}});
|
||||
|
||||
// Check simulated measurements.
|
||||
for (auto& unitTranslation : relativeTranslations) {
|
||||
EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
|
||||
unitTranslation.measured()));
|
||||
}
|
||||
|
||||
TranslationRecovery algorithm(relativeTranslations);
|
||||
const auto graph = algorithm.buildGraph();
|
||||
// Graph size will be zero as there no 'non-zero distance' edges.
|
||||
EXPECT_LONGS_EQUAL(0, graph.size());
|
||||
|
||||
// Run translation recovery
|
||||
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)));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
Loading…
Reference in New Issue