Fixed optimizer/marginals confusion
parent
2bd7a0ed43
commit
399a81911e
|
@ -19,7 +19,6 @@
|
||||||
#include <gtsam/slam/pose2SLAM.h>
|
#include <gtsam/slam/pose2SLAM.h>
|
||||||
|
|
||||||
// include this for marginals
|
// include this for marginals
|
||||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
|
||||||
#include <gtsam/nonlinear/Marginals.h>
|
#include <gtsam/nonlinear/Marginals.h>
|
||||||
|
|
||||||
#include <iomanip>
|
#include <iomanip>
|
||||||
|
@ -65,8 +64,7 @@ int main(int argc, char** argv) {
|
||||||
pose2SLAM::Values result = graph.optimize(initialEstimate);
|
pose2SLAM::Values result = graph.optimize(initialEstimate);
|
||||||
result.print("\nFinal result:\n ");
|
result.print("\nFinal result:\n ");
|
||||||
|
|
||||||
// use an explicit Optimizer object so we can query the marginals
|
// Query the marginals
|
||||||
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
|
|
||||||
Marginals marginals(graph, result);
|
Marginals marginals(graph, result);
|
||||||
cout.precision(2);
|
cout.precision(2);
|
||||||
cout << "\nP1:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(1)) << endl;
|
cout << "\nP1:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(1)) << endl;
|
||||||
|
|
|
@ -84,14 +84,14 @@ int main(int argc, char** argv) {
|
||||||
initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
|
initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
|
||||||
initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
|
initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
|
||||||
initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
|
initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
|
||||||
|
|
||||||
initialEstimate.print("\nInitial estimate:\n ");
|
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);
|
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
|
||||||
pose2SLAM::Values result = optimizer.optimize();
|
pose2SLAM::Values result = optimizer.optimize();
|
||||||
result.print("\nFinal result:\n ");
|
result.print("\nFinal result:\n ");
|
||||||
Values resultMultifrontal = optimizer.optimize();
|
|
||||||
|
// Query the marginals
|
||||||
Marginals marginals(graph, result);
|
Marginals marginals(graph, result);
|
||||||
cout.precision(2);
|
cout.precision(2);
|
||||||
cout << "\nP1:\n" << marginals.marginalCovariance(x1) << endl;
|
cout << "\nP1:\n" << marginals.marginalCovariance(x1) << endl;
|
||||||
|
|
Loading…
Reference in New Issue