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 // implementations for structures - needed if self-contained, and these should be included last
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam/nonlinear/Marginals.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -125,15 +124,13 @@ int main(int argc, char** argv) {
Values resultMultifrontal = optimizer.optimize(); Values resultMultifrontal = optimizer.optimize();
resultMultifrontal.print("final result (solved with a multifrontal solver)"); 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 marginals covariances for all variables
print(linearSolver.marginalCovariance(ordering[x1]), "x1 covariance"); Marginals marginals(graph, resultMultifrontal, Marginals::CHOLESKY);
print(linearSolver.marginalCovariance(ordering[x2]), "x2 covariance"); print(marginals.marginalCovariance(x1), "x1 covariance");
print(linearSolver.marginalCovariance(ordering[x3]), "x3 covariance"); print(marginals.marginalCovariance(x2), "x2 covariance");
print(linearSolver.marginalCovariance(ordering[l1]), "l1 covariance"); print(marginals.marginalCovariance(x3), "x3 covariance");
print(linearSolver.marginalCovariance(ordering[l2]), "l2 covariance"); print(marginals.marginalCovariance(l1), "l1 covariance");
print(marginals.marginalCovariance(l2), "l2 covariance");
return 0; 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 // pull in the Pose2 SLAM domain with all typedefs and helper functions defined
#include <gtsam/slam/pose2SLAM.h> #include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
@ -74,12 +75,13 @@ int main(int argc, char** argv) {
pose2SLAM::Values result = optimizer.optimize(); pose2SLAM::Values result = optimizer.optimize();
result.print("final result"); result.print("final result");
// /* Get covariances */ /* Get covariances */
// Matrix covariance1 = optimizer_result.marginalCovariance(PoseKey(1)); Marginals marginals(graph, result, Marginals::CHOLESKY);
// Matrix covariance2 = optimizer_result.marginalCovariance(PoseKey(2)); Matrix covariance1 = marginals.marginalCovariance(PoseKey(1));
// Matrix covariance2 = marginals.marginalCovariance(PoseKey(2));
// print(covariance1, "Covariance1");
// print(covariance2, "Covariance2"); print(covariance1, "Covariance1");
print(covariance2, "Covariance2");
return 0; return 0;
} }