12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Updated examples to use the new Marginals interface

release/4.3a0
Stephen Williams 2012-05-15 00:15:11 +00:00
parent 516e1610a1
commit ff3edc6823
2 changed files with 15 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -36,8 +36,7 @@
// implementations for structures - needed if self-contained, and these should be included last
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/nonlinear/Marginals.h>
using namespace std;
using namespace gtsam;
@ -125,15 +124,13 @@ int main(int argc, char** argv) {
Values resultMultifrontal = optimizer.optimize();
resultMultifrontal.print("final result (solved with a multifrontal solver)");
const Ordering& ordering = *optimizer.params().ordering;
GaussianMultifrontalSolver linearSolver(*graph.linearize(resultMultifrontal, ordering));
// Print marginals covariances for all variables
print(linearSolver.marginalCovariance(ordering[x1]), "x1 covariance");
print(linearSolver.marginalCovariance(ordering[x2]), "x2 covariance");
print(linearSolver.marginalCovariance(ordering[x3]), "x3 covariance");
print(linearSolver.marginalCovariance(ordering[l1]), "l1 covariance");
print(linearSolver.marginalCovariance(ordering[l2]), "l2 covariance");
Marginals marginals(graph, resultMultifrontal, Marginals::CHOLESKY);
print(marginals.marginalCovariance(x1), "x1 covariance");
print(marginals.marginalCovariance(x2), "x2 covariance");
print(marginals.marginalCovariance(x3), "x3 covariance");
print(marginals.marginalCovariance(l1), "l1 covariance");
print(marginals.marginalCovariance(l2), "l2 covariance");
return 0;
}

14
examples/Pose2SLAMExample_advanced.cpp
View File

@ -23,6 +23,7 @@
// pull in the Pose2 SLAM domain with all typedefs and helper functions defined
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
@ -74,12 +75,13 @@ int main(int argc, char** argv) {
pose2SLAM::Values result = optimizer.optimize();
result.print("final result");
// /* Get covariances */
// Matrix covariance1 = optimizer_result.marginalCovariance(PoseKey(1));
// Matrix covariance2 = optimizer_result.marginalCovariance(PoseKey(2));
//
// print(covariance1, "Covariance1");
// print(covariance2, "Covariance2");
/* Get covariances */
Marginals marginals(graph, result, Marginals::CHOLESKY);
Matrix covariance1 = marginals.marginalCovariance(PoseKey(1));
Matrix covariance2 = marginals.marginalCovariance(PoseKey(2));
print(covariance1, "Covariance1");
print(covariance2, "Covariance2");
return 0;
}