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

Removed SLAM namespace from testGradientDescentOptimizer

release/4.3a0
Stephen Williams 2012-07-23 22:44:02 +00:00
parent fb33b8a609
commit a641f599f6
1 changed files with 25 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
tests/testGradientDescentOptimizer.cpp
View File

@ -5,8 +5,12 @@
* @date Jun 11, 2012
*/
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/GradientDescentOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Pose2.h>
#include <CppUnitLite/TestHarness.h>
@ -19,34 +23,34 @@ using namespace std;
using namespace gtsam;
boost::tuple<pose2SLAM::Graph, Values> generateProblem() {
boost::tuple<NonlinearFactorGraph, Values> generateProblem() {
// 1. Create graph container and add factors to it
pose2SLAM::Graph graph ;
NonlinearFactorGraph graph ;
// 2a. Add Gaussian prior
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
graph.addPosePrior(1, priorMean, priorNoise);
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
// 2b. Add odometry factors
SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addRelativePose(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addRelativePose(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addRelativePose(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
// 2c. Add pose constraint
SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.addRelativePose(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty);
graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty));
// 3. Create the data structure to hold the initialEstimate estinmate to the solution
pose2SLAM::Values initialEstimate;
Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insertPose(1, x1);
Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insertPose(2, x2);
Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insertPose(3, x3);
Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insertPose(4, x4);
Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insertPose(5, x5);
Values initialEstimate;
Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insert(1, x1);
Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insert(2, x2);
Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insert(3, x3);
Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insert(4, x4);
Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insert(5, x5);
return boost::tie(graph, initialEstimate);
}
@ -55,8 +59,8 @@ boost::tuple<pose2SLAM::Graph, Values> generateProblem() {
/* ************************************************************************* */
TEST(optimize, GradientDescentOptimizer) {
pose2SLAM::Graph graph ;
pose2SLAM::Values initialEstimate;
NonlinearFactorGraph graph;
Values initialEstimate;
boost::tie(graph, initialEstimate) = generateProblem();
// cout << "initial error = " << graph.error(initialEstimate) << endl ;
@ -79,8 +83,8 @@ TEST(optimize, GradientDescentOptimizer) {
/* ************************************************************************* */
TEST(optimize, ConjugateGradientOptimizer) {
pose2SLAM::Graph graph ;
pose2SLAM::Values initialEstimate;
NonlinearFactorGraph graph;
Values initialEstimate;
boost::tie(graph, initialEstimate) = generateProblem();
// cout << "initial error = " << graph.error(initialEstimate) << endl ;
@ -102,8 +106,8 @@ TEST(optimize, ConjugateGradientOptimizer) {
/* ************************************************************************* */
TEST(optimize, GradientDescentOptimizer2) {
pose2SLAM::Graph graph ;
pose2SLAM::Values initialEstimate;
NonlinearFactorGraph graph;
Values initialEstimate;
boost::tie(graph, initialEstimate) = generateProblem();
// cout << "initial error = " << graph.error(initialEstimate) << endl ;