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

Added examples of using NonlinearOptimizer with multifrontal elimination

release/4.3a0
Richard Roberts 2010-10-22 02:53:27 +00:00
parent acde4d99a5
commit f354f8183d
3 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -38,6 +38,7 @@
#include <gtsam/nonlinear/TupleValues-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
// Main typedefs
typedef gtsam::TypedSymbol<gtsam::Pose2, 'x'> PoseKey; // Key for poses, with type included
@ -46,7 +47,8 @@ typedef gtsam::LieValues<PoseKey> PoseValues; // config type for
typedef gtsam::LieValues<PointKey> PointValues; // config type for points
typedef gtsam::TupleValues2<PoseValues, PointValues> Values; // main config with two variable classes
typedef gtsam::NonlinearFactorGraph<Values> Graph; // graph structure
typedef gtsam::NonlinearOptimizer<Graph,Values> Optimizer; // optimization engine for this domain
typedef gtsam::NonlinearOptimizer<Graph,Values,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential; // optimization engine for this domain
typedef gtsam::NonlinearOptimizer<Graph,Values,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> OptimizerMultifrontal; // optimization engine for this domain
using namespace std;
using namespace gtsam;
@ -120,9 +122,14 @@ int main(int argc, char** argv) {
initial.print("initial estimate");
// optimize using Levenburg-Marquadt optimization with an ordering from colamd
Optimizer::shared_values result = Optimizer::optimizeLM(graph, initial);
result->print("final result");
// first using sequential elimination
OptimizerSeqential::shared_values resultSequential = OptimizerSeqential::optimizeLM(graph, initial);
resultSequential->print("final result (solved with a sequential solver)");
// then using multifrontal
OptimizerMultifrontal::shared_values resultMultifrontal = OptimizerMultifrontal::optimizeLM(graph, initial);
resultMultifrontal->print("final result (solved with a multifrontal solver)");
return 0;
}

2
linear/NoiseModel.cpp
View File

@ -220,9 +220,7 @@ SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<std::vector<long>&> firs
// General QR, see also special version in Constrained
SharedDiagonal Gaussian::QRColumnWise(ublas::matrix<double, ublas::column_major>& Ab, vector<long>& firstZeroRows) const {
Matrix Abresult(Ab);
gtsam::print(Abresult, "Abresult before = ");
SharedDiagonal result = QR(Abresult, firstZeroRows);
gtsam::print(Abresult, "Abresult after = ");
Ab = Abresult;
return result;
// // get size(A) and maxRank

5
slam/pose2SLAM.h
View File

@ -26,6 +26,8 @@
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
namespace gtsam {
@ -60,7 +62,8 @@ namespace gtsam {
};
// Optimizer
typedef NonlinearOptimizer<Graph, Values> Optimizer;
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, GaussianSequentialSolver> OptimizerSequential;
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
} // pose2SLAM