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

planar SLAM example works in MATLAB !

release/4.3a0
Frank Dellaert 2011-10-22 20:10:25 +00:00
parent 7c55724ddd
commit 3fbc459d0f
5 changed files with 11 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
examples/PlanarSLAMExample_easy.cpp
View File

@ -86,10 +86,8 @@ int main(int argc, char** argv) {
initialEstimate.print("initial estimate");
// optimize using Levenberg-Marquardt optimization with an ordering from colamd
// Values result = optimize<Graph, Values>(graph, initialEstimate);
// result.print("final result");
boost::shared_ptr<Values> result = graph.optimize(initialEstimate);
result->print("final result");
Values result = optimize<Graph, Values>(graph, initialEstimate);
result.print("final result");
return 0;
}

2
examples/matlab/PlanarSLAMExample_easy.m
View File

@ -71,5 +71,5 @@ initialEstimate.insertPoint(l2, Landmark2(4.1, 1.8));
initialEstimate.print('initial estimate');
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
result = graph.optimize_(initialEstimate);
result.print('final result');

2
gtsam/slam/PlanarSLAMValues.h
View File

@ -11,7 +11,7 @@
/**
* @file PlanarSLAMValues.h
* @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
* @brief Convenience for MATLAB wrapper, which does not allow for identically named methods
* @author Frank Dellaert
*/

1
gtsam/slam/planarSLAM.cpp
View File

@ -26,7 +26,6 @@ namespace gtsam {
using namespace planarSLAM;
INSTANTIATE_LIE_VALUES(PointKey)
INSTANTIATE_LIE_VALUES(PoseKey)
INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values)

13
gtsam/slam/planarSLAM.h
View File

@ -68,11 +68,11 @@ namespace gtsam {
/// insert a pose
void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); }
/// insert a point
/// insert a point
void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); }
};
/**
/**
* List of typedefs for factors
*/
@ -120,12 +120,13 @@ namespace gtsam {
void addBearingRange(const PoseKey& i, const PointKey& j,
const Rot2& z1, double z2, const SharedNoiseModel& model);
/// Optimize, mostly here for MATLAB
boost::shared_ptr<Values> optimize(const Values& initialEstimate) {
/// Optimize_ for MATLAB
boost::shared_ptr<Values> optimize_(const Values& initialEstimate) {
typedef NonlinearOptimizer<Graph, Values> Optimizer;
// NonlinearOptimizationParameters::LAMBDA
boost::shared_ptr<Values> result(
new Values(*Optimizer::optimizeGN(*this, initialEstimate)));
new Values(
*Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA)));
return result;
}
};