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"); initialEstimate.print("initial estimate");
// optimize using Levenberg-Marquardt optimization with an ordering from colamd // optimize using Levenberg-Marquardt optimization with an ordering from colamd
// Values result = optimize<Graph, Values>(graph, initialEstimate); Values result = optimize<Graph, Values>(graph, initialEstimate);
// result.print("final result"); result.print("final result");
boost::shared_ptr<Values> result = graph.optimize(initialEstimate);
result->print("final result");
return 0; 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'); initialEstimate.print('initial estimate');
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate); result = graph.optimize_(initialEstimate);
result.print('final result'); result.print('final result');

2
gtsam/slam/PlanarSLAMValues.h
View File

@ -11,7 +11,7 @@
/** /**
* @file PlanarSLAMValues.h * @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 * @author Frank Dellaert
*/ */

1
gtsam/slam/planarSLAM.cpp
View File

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

13
gtsam/slam/planarSLAM.h
View File

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