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

Updated examples for new NonlinearOptimizer

release/4.3a0
Richard Roberts 2012-03-24 19:53:17 +00:00
parent 31fe933877
commit 7a24e1c940
7 changed files with 38 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/CameraResectioning.cpp
View File

@ -22,7 +22,7 @@
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
using namespace gtsam;
@ -95,7 +95,7 @@ int main(int argc, char* argv[]) {
0.,0.,-1.), Point3(0.,0.,2.0)));
/* 4. Optimize the graph using Levenberg-Marquardt*/
Values result = optimize<NonlinearFactorGraph> (graph, initial);
Values result = *LevenbergMarquardtOptimizer(graph, initial).optimized();
result.print("Final result: ");
return 0;

4
examples/PlanarSLAMExample_easy.cpp
View File

@ -20,7 +20,7 @@
// pull in the planar SLAM domain with all typedefs and helper functions defined
#include <gtsam/slam/planarSLAM.h>
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
using namespace std;
using namespace gtsam;
@ -83,7 +83,7 @@ int main(int argc, char** argv) {
initialEstimate.print("initial estimate");
// optimize using Levenberg-Marquardt optimization with an ordering from colamd
planarSLAM::Values result = optimize(graph, initialEstimate);
planarSLAM::Values result = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimized();
result.print("final result");
return 0;

33
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -35,17 +35,13 @@
// implementations for structures - needed if self-contained, and these should be included last
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
using namespace std;
using namespace gtsam;
// Main typedefs
typedef NonlinearOptimizer<NonlinearFactorGraph,GaussianFactorGraph,GaussianSequentialSolver> OptimizerSeqential; // optimization engine for this domain
typedef NonlinearOptimizer<NonlinearFactorGraph,GaussianFactorGraph,GaussianMultifrontalSolver> OptimizerMultifrontal; // optimization engine for this domain
/**
* In this version of the system we make the following assumptions:
* - All values are axis aligned
@ -117,22 +113,27 @@ int main(int argc, char** argv) {
// optimize using Levenberg-Marquardt optimization with an ordering from colamd
// first using sequential elimination
OptimizerSeqential::shared_values resultSequential = OptimizerSeqential::optimizeLM(*graph, *initial);
LevenbergMarquardtParams lmParams;
lmParams.elimination = LevenbergMarquardtParams::SEQUENTIAL;
Values::const_shared_ptr resultSequential = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimized();
resultSequential->print("final result (solved with a sequential solver)");
// then using multifrontal, advanced interface
// Note how we create an optimizer, call LM, then we get access to covariances
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initial);
OptimizerMultifrontal optimizerMF(graph, initial, ordering);
OptimizerMultifrontal resultMF = optimizerMF.levenbergMarquardt();
resultMF.values()->print("final result (solved with a multifrontal solver)");
// Note that we keep the original optimizer object so we can use the COLAMD
// ordering it computes.
LevenbergMarquardtOptimizer optimizer(graph, initial);
Values::const_shared_ptr resultMultifrontal = optimizer.optimized();
resultMultifrontal->print("final result (solved with a multifrontal solver)");
const Ordering& ordering = *optimizer.ordering();
GaussianMultifrontalSolver linearSolver(*graph->linearize(*resultMultifrontal, ordering));
// Print marginals covariances for all variables
print(resultMF.marginalCovariance(x1), "x1 covariance");
print(resultMF.marginalCovariance(x2), "x2 covariance");
print(resultMF.marginalCovariance(x3), "x3 covariance");
print(resultMF.marginalCovariance(l1), "l1 covariance");
print(resultMF.marginalCovariance(l2), "l2 covariance");
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");
return 0;
}

16
examples/Pose2SLAMExample_advanced.cpp
View File

@ -22,7 +22,7 @@
// pull in the Pose2 SLAM domain with all typedefs and helper functions defined
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
@ -65,16 +65,16 @@ int main(int argc, char** argv) {
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initial);
/* 4.2.2 set up solver and optimize */
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
Optimizer optimizer(graph, initial, ordering, params);
Optimizer optimizer_result = optimizer.levenbergMarquardt();
pose2SLAM::Values result = *optimizer_result.values();
LevenbergMarquardtParams params;
params.relativeErrorTol = 1e-15;
params.absoluteErrorTol = 1e-15;
pose2SLAM::Values result = *LevenbergMarquardtOptimizer(graph, initial, params, ordering).optimized();
result.print("final result");
/* Get covariances */
Matrix covariance1 = optimizer_result.marginalCovariance(PoseKey(1));
Matrix covariance2 = optimizer_result.marginalCovariance(PoseKey(1));
GaussianMultifrontalSolver solver(*graph->linearize(result, *ordering));
Matrix covariance1 = solver.marginalCovariance(ordering->at(PoseKey(1)));
Matrix covariance2 = solver.marginalCovariance(ordering->at(PoseKey(1)));
print(covariance1, "Covariance1");
print(covariance2, "Covariance2");

4
examples/Pose2SLAMExample_easy.cpp
View File

@ -24,7 +24,7 @@
// pull in the Pose2 SLAM domain with all typedefs and helper functions defined
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
using namespace std;
using namespace gtsam;
@ -61,7 +61,7 @@ int main(int argc, char** argv) {
/* 4 Single Step Optimization
* optimize using Levenberg-Marquardt optimization with an ordering from colamd */
pose2SLAM::Values result = optimize<Graph>(graph, initial);
pose2SLAM::Values result = *LevenbergMarquardtOptimizer(graph, initial).optimized();
result.print("final result");

4
examples/SimpleRotation.cpp
View File

@ -24,7 +24,7 @@
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
/*
* TODO: make factors independent of RotValues
@ -105,7 +105,7 @@ int main() {
* initial estimate. This will yield a new RotValues structure
* with the final state of the optimization.
*/
Values result = optimize(graph, initial);
Values result = *LevenbergMarquardtOptimizer(graph, initial).optimized();
result.print("final result");
return 0;

9
examples/vSLAMexample/vSFMexample.cpp
View File

@ -20,7 +20,7 @@
using namespace boost;
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/PriorFactor.h>
@ -143,12 +143,13 @@ int main(int argc, char* argv[]) {
// Optimize the graph
cout << "*******************************************************" << endl;
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newVerbosity(Optimizer::Parameters::DAMPED);
visualSLAM::Optimizer::shared_values result = visualSLAM::Optimizer::optimizeGN(graph, initialEstimates, params);
LevenbergMarquardtParams params;
params.lmVerbosity = LevenbergMarquardtParams::DAMPED;
visualSLAM::Values result = *LevenbergMarquardtOptimizer(graph, initialEstimates, params).optimized();
// Print final results
cout << "*******************************************************" << endl;
result->print("FINAL RESULTS: ");
result.print("FINAL RESULTS: ");
return 0;
}