marginals
							parent
							
								
									00c56b0d0a
								
							
						
					
					
						commit
						7a28e6d5cb
					
				|  | @ -18,6 +18,13 @@ | |||
| // pull in the 2D PoseSLAM domain with all typedefs and helper functions defined
 | ||||
| #include <gtsam/slam/pose2SLAM.h> | ||||
| 
 | ||||
| // include this for marginals
 | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| #include <gtsam/nonlinear/Marginals.h> | ||||
| 
 | ||||
| #include <iomanip> | ||||
| #include <iostream> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
|  | @ -58,6 +65,14 @@ int main(int argc, char** argv) { | |||
| 	pose2SLAM::Values result = graph.optimize(initialEstimate); | ||||
| 	result.print("\nFinal result:\n  "); | ||||
| 
 | ||||
| 	// use an explicit Optimizer object so we can query the marginals
 | ||||
| 	LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); | ||||
| 	Marginals marginals(graph, result); | ||||
| 	cout.precision(2); | ||||
|   cout << "\nP1:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(1)) << endl; | ||||
|   cout << "\nP2:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(2)) << endl; | ||||
|   cout << "\nP3:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(3)) << endl; | ||||
| 
 | ||||
| 	return 0; | ||||
| } | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue