Added iSAM2 test to check issue #412
							parent
							
								
									63d7d7c54b
								
							
						
					
					
						commit
						ab0fb0d4a5
					
				|  | @ -0,0 +1,125 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||||
|  * Atlanta, Georgia 30332-0415 | ||||
|  * All Rights Reserved | ||||
|  * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  * @file    testVisualISAM2.cpp | ||||
|  * @brief   Test convergence of visualSLAM example. | ||||
|  * @author  Duy-Nguyen Ta | ||||
|  * @author  Frank Dellaert | ||||
|  */ | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| #include <examples/SFMdata.h> | ||||
| #include <gtsam/geometry/Point2.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/nonlinear/ISAM2.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/nonlinear/Values.h> | ||||
| #include <gtsam/slam/PriorFactor.h> | ||||
| #include <gtsam/slam/ProjectionFactor.h> | ||||
| 
 | ||||
| #include <vector> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(testVisualISAM2, all) | ||||
| { | ||||
|     Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); | ||||
| 
 | ||||
|     auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); | ||||
| 
 | ||||
|     vector<Point3> points = createPoints(); | ||||
| 
 | ||||
|     // Create the set of ground-truth poses
 | ||||
|     vector<Pose3> poses = createPoses(); | ||||
| 
 | ||||
|     // Set the parameters
 | ||||
|     ISAM2Params parameters; | ||||
|     parameters.relinearizeThreshold = 0.01; | ||||
|     parameters.relinearizeSkip = 1; | ||||
|     ISAM2 isam(parameters); | ||||
| 
 | ||||
|     // Create a Factor Graph and Values to hold the new data
 | ||||
|     NonlinearFactorGraph graph; | ||||
|     Values initialEstimate; | ||||
| 
 | ||||
|     // Loop over the poses, adding the observations to iSAM incrementally
 | ||||
|     for (size_t i = 0; i < poses.size(); ++i) | ||||
|     { | ||||
|         // Add factors for each landmark observation
 | ||||
|         for (size_t j = 0; j < points.size(); ++j) | ||||
|         { | ||||
|             SimpleCamera camera(poses[i], *K); | ||||
|             Point2 measurement = camera.project(points[j]); | ||||
|             graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>( | ||||
|                 measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); | ||||
|         } | ||||
| 
 | ||||
|         // Add an initial guess for the current pose
 | ||||
|         // Intentionally initialize the variables off from the ground truth
 | ||||
|         static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25), | ||||
|                                 Point3(0.05, -0.10, 0.20)); | ||||
|         initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose); | ||||
| 
 | ||||
|         // Treat first iteration as special case
 | ||||
|         if (i == 0) | ||||
|         { | ||||
|             // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw
 | ||||
|             static auto kPosePrior = noiseModel::Diagonal::Sigmas( | ||||
|                 (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)) | ||||
|                     .finished()); | ||||
|             graph.emplace_shared<PriorFactor<Pose3>>(Symbol('x', 0), poses[0], | ||||
|                                                      kPosePrior); | ||||
| 
 | ||||
|             // Add a prior on landmark l0
 | ||||
|             static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1); | ||||
|             graph.emplace_shared<PriorFactor<Point3>>(Symbol('l', 0), points[0], | ||||
|                                                       kPointPrior); | ||||
| 
 | ||||
|             // Add initial guesses to all observed landmarks
 | ||||
|             // Intentionally initialize the variables off from the ground truth
 | ||||
|             static Point3 kDeltaPoint(-0.25, 0.20, 0.15); | ||||
|             for (size_t j = 0; j < points.size(); ++j) | ||||
|                 initialEstimate.insert<Point3>(Symbol('l', j), points[j] + kDeltaPoint); | ||||
|         } | ||||
|         else | ||||
|         { | ||||
|             // Update iSAM with the new factors
 | ||||
|             isam.update(graph, initialEstimate); | ||||
| 
 | ||||
|             // Optimize
 | ||||
|             Values currentEstimate = isam.calculateEstimate(); | ||||
| 
 | ||||
|             // reset for next iteration
 | ||||
|             graph.resize(0); | ||||
|             initialEstimate.clear(); | ||||
|         } | ||||
|     } // for loop
 | ||||
| 
 | ||||
|     auto result = isam.calculateEstimate(); | ||||
|     EXPECT_LONGS_EQUAL(16, result.size()); | ||||
|     for (size_t j = 0; j < points.size(); ++j) | ||||
|     { | ||||
|         Symbol key('l', j); | ||||
|         EXPECT(assert_equal(points[j], result.at<Point3>(key), 0.01)); | ||||
|     } | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() | ||||
| { | ||||
|     TestResult tr; | ||||
|     return TestRegistry::runAllTests(tr); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
		Loading…
	
		Reference in New Issue