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