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

Fixing Pose2SLAMwSPCG and make a unittest in testPose2SLAM for optimizeSPCG(). Why does SPCG need a constraint between 

2 poses to work? GaussNewton still works fine without the constraint!
release/4.3a0
Duy-Nguyen Ta 2012-06-22 23:40:04 +00:00
parent b7a13c7061
commit eecafcebe7
4 changed files with 42 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
examples/Pose2SLAMwSPCG.cpp
View File

@ -85,5 +85,10 @@ int main(void) {
cout << "subgraph solver final error = " << graph.error(result) << endl;
}
{
Values result = graph.optimizeSPCG(initialEstimate);
result.print("\nFinal result:\n");
}
return 0 ;
}

2
gtsam/slam/pose2SLAM.cpp
View File

@ -15,6 +15,7 @@
* @author Frank Dellaert
**/
#include <gtsam/linear/SimpleSPCGSolver.h>
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -88,6 +89,7 @@ namespace pose2SLAM {
Values Graph::optimizeSPCG(const Values& initialEstimate) const {
LevenbergMarquardtParams params;
params.linearSolverType = SuccessiveLinearizationParams::CG;
params.iterativeParams = boost::make_shared<SimpleSPCGSolverParameters>();
return LevenbergMarquardtOptimizer(*this, initialEstimate, params).optimize();
}

31
gtsam/slam/tests/testPose2SLAM.cpp
View File

@ -17,6 +17,7 @@
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/base/numericalDerivative.h>
using namespace gtsam;
@ -190,6 +191,36 @@ TEST_UNSAFE(Pose2SLAM, optimize) {
EQUALITY(expectedP1, actualP1);
}
/* ************************************************************************* */
TEST_UNSAFE(Pose2SLAM, optimizeSPCG) {
// create a Pose graph with one equality constraint and one measurement
pose2SLAM::Graph fg;
fg.addPrior(0, Pose2(0,0,0), noiseModel::Diagonal::Sigmas(Vector_(3,3.0,3.0,1.0)));
fg.addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance);
// [Duy] For some unknown reason, SPCG needs this constraint to work. GaussNewton doesn't need this.
fg.addConstraint(0, 1, Pose2(1,2,M_PI_2), noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)));
// Create initial config
Values initial;
initial.insert(0, Pose2(0,0,0));
initial.insert(1, Pose2(0,0,0));
// Optimize using SPCG
Values actual = fg.optimizeSPCG(initial);
// Try GaussNewton without the above constraint to see if the problem is underconstrained. Still works!
Values actual2 = GaussNewtonOptimizer(fg, initial).optimize();
// Check with expected config
Values expected;
expected.insert(0, Pose2(0,0,0));
expected.insert(1, Pose2(1,2,M_PI_2));
CHECK(assert_equal(expected, actual));
CHECK(assert_equal(expected, actual2));
}
/* ************************************************************************* */
// test optimization with 3 poses, note we use plain Keys here, not symbols
TEST_UNSAFE(Pose2SLAM, optimizeThreePoses) {

4
matlab/tests/testPose2SLAMExample.m
View File

@ -49,6 +49,7 @@ initialEstimate.insertPose(5, gtsamPose2(2.1, 2.1,-pi/2));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
resultSPCG = graph.optimizeSPCG(initialEstimate);
%% Plot Covariance Ellipses
marginals = graph.marginals(result);
@ -57,4 +58,7 @@ P = marginals.marginalCovariance(1);
pose_1 = result.pose(1);
CHECK('pose_1.equals(gtsamPose2,1e-4)',pose_1.equals(gtsamPose2,1e-4));
poseSPCG_1 = resultSPCG.pose(1);
CHECK('poseSPCG_1.equals(gtsamPose2,1e-4)',poseSPCG_1.equals(gtsamPose2,1e-4));