Fixed optimizer/marginals confusion

release/4.3a0
Frank Dellaert 2012-05-21 21:52:50 +00:00
parent 2bd7a0ed43
commit 399a81911e
2 changed files with 4 additions and 6 deletions

View File

@ -19,7 +19,6 @@
#include <gtsam/slam/pose2SLAM.h>
// include this for marginals
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <iomanip>
@ -65,8 +64,7 @@ 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);
// Query the marginals
Marginals marginals(graph, result);
cout.precision(2);
cout << "\nP1:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(1)) << endl;

View File

@ -84,14 +84,14 @@ int main(int argc, char** argv) {
initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
initialEstimate.print("\nInitial estimate:\n ");
// use an explicit Optimizer object, used for both optimization and marginal inference
// use an explicit Optimizer object
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
pose2SLAM::Values result = optimizer.optimize();
result.print("\nFinal result:\n ");
Values resultMultifrontal = optimizer.optimize();
// Query the marginals
Marginals marginals(graph, result);
cout.precision(2);
cout << "\nP1:\n" << marginals.marginalCovariance(x1) << endl;