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

refactored Pose2SLAM

release/4.3a0
Chris Beall 2012-01-28 02:31:44 +00:00
parent ba34a43b1c
commit 63dc9399b9
12 changed files with 166 additions and 150 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
examples/Pose2SLAMExample_advanced.cpp
View File

@ -34,7 +34,7 @@ using namespace gtsam::pose2SLAM;
int main(int argc, char** argv) {
// create keys for robot positions
Key x1(1), x2(2), x3(3);
PoseKey x1(1), x2(2), x3(3);
/* 1. create graph container and add factors to it */
shared_ptr<Graph> graph(new Graph);
@ -51,8 +51,8 @@ int main(int argc, char** argv) {
/* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
graph->addConstraint(x1, x2, odom_measurement, odom_model);
graph->addConstraint(x2, x3, odom_measurement, odom_model);
graph->addOdometry(x1, x2, odom_measurement, odom_model);
graph->addOdometry(x2, x3, odom_measurement, odom_model);
graph->print("full graph");
/* 3. Create the data structure to hold the initial estimate to the solution

6
examples/Pose2SLAMExample_easy.cpp
View File

@ -32,7 +32,7 @@ using namespace gtsam::pose2SLAM;
int main(int argc, char** argv) {
// create keys for robot positions
Key x1(1), x2(2), x3(3);
PoseKey x1(1), x2(2), x3(3);
/* 1. create graph container and add factors to it */
Graph graph ;
@ -49,8 +49,8 @@ int main(int argc, char** argv) {
/* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
graph.addConstraint(x1, x2, odom_measurement, odom_model);
graph.addConstraint(x2, x3, odom_measurement, odom_model);
graph.addOdometry(x1, x2, odom_measurement, odom_model);
graph.addOdometry(x2, x3, odom_measurement, odom_model);
graph.print("full graph");
/* 3. Create the data structure to hold the initial estinmate to the solution

19
gtsam/slam/dataset.cpp
View File

@ -28,8 +28,9 @@ using namespace gtsam;
#define LINESIZE 81920
typedef boost::shared_ptr<Pose2Graph> sharedPose2Graph;
typedef boost::shared_ptr<Pose2Values> sharedPose2Values;
typedef boost::shared_ptr<pose2SLAM::Graph> sharedPose2Graph;
typedef boost::shared_ptr<pose2SLAM::Values> sharedPose2Values;
typedef pose2SLAM::Odometry Pose2Factor; ///< Typedef for Constraint class for backwards compatibility
namespace gtsam {
@ -80,8 +81,8 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
exit(-1);
}
sharedPose2Values poses(new Pose2Values);
sharedPose2Graph graph(new Pose2Graph);
sharedPose2Values poses(new pose2SLAM::Values);
sharedPose2Graph graph(new pose2SLAM::Graph);
string tag;
@ -135,7 +136,7 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
if (!poses->exists(id1)) poses->insert(id1, Pose2());
if (!poses->exists(id2)) poses->insert(id2, poses->at(id1) * l1Xl2);
Pose2Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model));
pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model));
graph->push_back(factor);
}
is.ignore(LINESIZE, '\n');
@ -148,14 +149,14 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
}
/* ************************************************************************* */
void save2D(const Pose2Graph& graph, const Pose2Values& config, const SharedDiagonal model,
void save2D(const pose2SLAM::Graph& graph, const pose2SLAM::Values& config, const SharedDiagonal model,
const string& filename) {
typedef Pose2Values::Key Key;
typedef pose2SLAM::Values::Key Key;
fstream stream(filename.c_str(), fstream::out);
// save poses
Pose2Values::Key key;
pose2SLAM::Values::Key key;
Pose2 pose;
BOOST_FOREACH(boost::tie(key, pose), config)
stream << "VERTEX2 " << key.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl;
@ -163,7 +164,7 @@ void save2D(const Pose2Graph& graph, const Pose2Values& config, const SharedDiag
// save edges
Matrix R = model->R();
Matrix RR = trans(R)*R;//prod(trans(R),R);
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor<Pose2Values> > factor_, graph) {
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor<pose2SLAM::Values> > factor_, graph) {
boost::shared_ptr<Pose2Factor> factor = boost::dynamic_pointer_cast<Pose2Factor>(factor_);
if (!factor) continue;

6
gtsam/slam/dataset.h
View File

@ -40,16 +40,16 @@ std::pair<std::string, boost::optional<gtsam::SharedDiagonal> >
* @param maxID, if non-zero cut out vertices >= maxID
* @param smart: try to reduce complexity of covariance to cheapest model
*/
std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<gtsam::Pose2Values> > load2D(
std::pair<boost::shared_ptr<pose2SLAM::Graph>, boost::shared_ptr<pose2SLAM::Values> > load2D(
std::pair<std::string, boost::optional<SharedDiagonal> > dataset,
int maxID = 0, bool addNoise=false, bool smart=true);
std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<gtsam::Pose2Values> > load2D(
std::pair<boost::shared_ptr<pose2SLAM::Graph>, boost::shared_ptr<pose2SLAM::Values> > load2D(
const std::string& filename,
boost::optional<gtsam::SharedDiagonal> model = boost::optional<gtsam::SharedDiagonal>(),
int maxID = 0, bool addNoise=false, bool smart=true);
/** save 2d graph */
void save2D(const gtsam::Pose2Graph& graph, const gtsam::Pose2Values& config, const gtsam::SharedDiagonal model,
void save2D(const pose2SLAM::Graph& graph, const pose2SLAM::Values& config, const gtsam::SharedDiagonal model,
const std::string& filename);
/**

18
gtsam/slam/pose2SLAM.cpp
View File

@ -11,7 +11,7 @@
/**
* @file pose2SLAM.cpp
* @brief: bearing/range measurements in 2D plane
* @brief: Odometry measurements in 2D plane
* @author Frank Dellaert
**/
@ -36,20 +36,20 @@ namespace gtsam {
}
/* ************************************************************************* */
void Graph::addPrior(const Key& i, const Pose2& p,
void Graph::addPrior(const PoseKey& i, const Pose2& p,
const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model));
push_back(factor);
}
void Graph::addConstraint(const Key& i, const Key& j, const Pose2& z,
const SharedNoiseModel& model) {
sharedFactor factor(new Constraint(i, j, z, model));
push_back(factor);
}
void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) {
sharedFactor factor(new HardConstraint(i, p));
push_back(factor);
}
void Graph::addHardConstraint(const Key& i, const Pose2& p) {
sharedFactor factor(new HardConstraint(i, p));
void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(i, j, z, model));
push_back(factor);
}

105
gtsam/slam/pose2SLAM.h
View File

@ -17,7 +17,6 @@
#pragma once
#include <gtsam/base/LieVector.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
@ -31,67 +30,81 @@
namespace gtsam {
// Use pose2SLAM namespace for specific SLAM instance
namespace pose2SLAM {
// Use pose2SLAM namespace for specific SLAM instance
namespace pose2SLAM {
/// Keys with Pose2 and symbol 'x'
typedef TypedSymbol<Pose2, 'x'> Key;
/// Keys with Pose2 and symbol 'x'
typedef TypedSymbol<Pose2, 'x'> PoseKey;
/// Values with type 'Key'
typedef Values<Key> Values;
/// Values class, inherited from Values, using PoseKeys
struct Values: public gtsam::Values<PoseKey> {
/**
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
* @param n number of poses
* @param R radius of circle
* @param c character to use for keys
* @return circle of n 2D poses
*/
Values circle(size_t n, double R);
/// Default constructor
Values() {}
/// A prior factor on Key with Pose2 data type.
typedef PriorFactor<Values, Key> Prior;
/// Copy constructor
Values(const gtsam::Values<PoseKey>& values) :
gtsam::Values<PoseKey>(values) {
}
/// A factor to put constraints between two factors.
typedef BetweenFactor<Values, Key> Constraint;
// Convenience for MATLAB wrapper, which does not allow for identically named methods
/// A hard constraint would enforce that the given key would have the input value in the results.
typedef NonlinearEquality<Values, Key> HardConstraint;
/// get a pose
Pose2 pose(int key) const { return (*this)[PoseKey(key)]; }
/// Graph
struct Graph: public NonlinearFactorGraph<Values> {
/// insert a pose
void insertPose(int key, const Pose2& pose) { insert(PoseKey(key), pose); }
};
/// Adds a factor between keys of the same type
typedef BetweenFactor<Values, Key> Constraint;
/**
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
* @param n number of poses
* @param R radius of circle
* @param c character to use for keys
* @return circle of n 2D poses
*/
Values circle(size_t n, double R);
/// A simple typedef from Pose2 to Pose
typedef Pose2 Pose;
/**
* List of typedefs for factors
*/
/// Adds a Pose2 prior with a noise model to one of the keys in the nonlinear factor graph
void addPrior(const Key& i, const Pose2& p, const SharedNoiseModel& model);
/// A hard constraint to enforce a specific value for a pose
typedef NonlinearEquality<Values, PoseKey> HardConstraint;
/// A prior factor on a pose with Pose2 data type.
typedef PriorFactor<Values, PoseKey> Prior;
/// A factor to add an odometry measurement between two poses.
typedef BetweenFactor<Values, PoseKey> Odometry;
/// Creates a between factor between keys i and j with a noise model with Pose2 z in the graph
void addConstraint(const Key& i, const Key& j, const Pose2& z, const SharedNoiseModel& model);
/// Graph
struct Graph: public NonlinearFactorGraph<Values> {
/// Creates a hard constraint for key i with the given Pose2 p.
void addHardConstraint(const Key& i, const Pose2& p);
};
/// Default constructor for a NonlinearFactorGraph
Graph(){}
/// The sequential optimizer
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, GaussianSequentialSolver> OptimizerSequential;
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
Graph(const NonlinearFactorGraph<Values>& graph);
/// The multifrontal optimizer
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
/// Adds a Pose2 prior with a noise model to one of the keys in the nonlinear factor graph
void addPrior(const PoseKey& i, const Pose2& p, const SharedNoiseModel& model);
} // pose2SLAM
/// Creates a hard constraint for key i with the given Pose2 p.
void addPoseConstraint(const PoseKey& i, const Pose2& p);
/**
* Backwards compatibility
*/
typedef pose2SLAM::Values Pose2Values; ///< Typedef for Values class for backwards compatibility
typedef pose2SLAM::Prior Pose2Prior; ///< Typedef for Prior class for backwards compatibility
typedef pose2SLAM::Constraint Pose2Factor; ///< Typedef for Constraint class for backwards compatibility
typedef pose2SLAM::Graph Pose2Graph; ///< Typedef for Graph class for backwards compatibility
/// Creates a between factor between keys i and j with a noise model with Pose2 z in the graph
void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
const SharedNoiseModel& model);
};
/// The sequential optimizer
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph,
GaussianSequentialSolver> OptimizerSequential;
/// The multifrontal optimizer
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph,
GaussianMultifrontalSolver> Optimizer;
} // pose2SLAM
} // namespace gtsam

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

@ -33,6 +33,8 @@ using namespace boost::assign;
#include <iostream>
using namespace std;
typedef pose2SLAM::Odometry Pose2Factor;
// common measurement covariance
static double sx=0.5, sy=0.5,st=0.1;
static noiseModel::Gaussian::shared_ptr covariance(
@ -94,8 +96,8 @@ TEST( Pose2SLAM, constructor )
{
// create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2);
Pose2Graph graph;
graph.addConstraint(1,2,measured, covariance);
pose2SLAM::Graph graph;
graph.addOdometry(1,2,measured, covariance);
// get the size of the graph
size_t actual = graph.size();
// verify
@ -109,13 +111,13 @@ TEST( Pose2SLAM, linearization )
// create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2);
Pose2Factor constraint(1,2,measured, covariance);
Pose2Graph graph;
graph.addConstraint(1,2,measured, covariance);
pose2SLAM::Graph graph;
graph.addOdometry(1,2,measured, covariance);
// Choose a linearization point
Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2)
Pose2Values config;
pose2SLAM::Values config;
config.insert(1,p1);
config.insert(2,p2);
// Linearize
@ -146,26 +148,26 @@ TEST( Pose2SLAM, linearization )
TEST(Pose2Graph, optimize) {
// create a Pose graph with one equality constraint and one measurement
shared_ptr<Pose2Graph> fg(new Pose2Graph);
fg->addHardConstraint(0, Pose2(0,0,0));
fg->addConstraint(0, 1, Pose2(1,2,M_PI_2), covariance);
shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph);
fg->addPoseConstraint(0, Pose2(0,0,0));
fg->addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance);
// Create initial config
boost::shared_ptr<Pose2Values> initial(new Pose2Values());
boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values());
initial->insert(0, Pose2(0,0,0));
initial->insert(1, Pose2(0,0,0));
// Choose an ordering and optimize
shared_ptr<Ordering> ordering(new Ordering);
*ordering += "x0","x1";
typedef NonlinearOptimizer<Pose2Graph, Pose2Values> Optimizer;
typedef NonlinearOptimizer<pose2SLAM::Graph, pose2SLAM::Values> Optimizer;
NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15);
Optimizer optimizer0(fg, initial, ordering, params);
Optimizer optimizer = optimizer0.levenbergMarquardt();
// Check with expected config
Pose2Values expected;
pose2SLAM::Values expected;
expected.insert(0, Pose2(0,0,0));
expected.insert(1, Pose2(1,2,M_PI_2));
CHECK(assert_equal(expected, *optimizer.values()));
@ -176,19 +178,19 @@ TEST(Pose2Graph, optimize) {
TEST(Pose2Graph, optimizeThreePoses) {
// Create a hexagon of poses
Pose2Values hexagon = pose2SLAM::circle(3,1.0);
pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0);
Pose2 p0 = hexagon[0], p1 = hexagon[1];
// create a Pose graph with one equality constraint and one measurement
shared_ptr<Pose2Graph> fg(new Pose2Graph);
fg->addHardConstraint(0, p0);
shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph);
fg->addPoseConstraint(0, p0);
Pose2 delta = p0.between(p1);
fg->addConstraint(0, 1, delta, covariance);
fg->addConstraint(1, 2, delta, covariance);
fg->addConstraint(2, 0, delta, covariance);
fg->addOdometry(0, 1, delta, covariance);
fg->addOdometry(1, 2, delta, covariance);
fg->addOdometry(2, 0, delta, covariance);
// Create initial config
boost::shared_ptr<Pose2Values> initial(new Pose2Values());
boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values());
initial->insert(0, p0);
initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1)));
@ -202,7 +204,7 @@ TEST(Pose2Graph, optimizeThreePoses) {
pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params);
pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
Pose2Values actual = *optimizer.values();
pose2SLAM::Values actual = *optimizer.values();
// Check with ground truth
CHECK(assert_equal(hexagon, actual));
@ -210,25 +212,25 @@ TEST(Pose2Graph, optimizeThreePoses) {
/* ************************************************************************* */
// test optimization with 6 poses arranged in a hexagon and a loop closure
TEST_UNSAFE(Pose2Graph, optimizeCircle) {
TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
// Create a hexagon of poses
Pose2Values hexagon = pose2SLAM::circle(6,1.0);
pose2SLAM::Values hexagon = pose2SLAM::circle(6,1.0);
Pose2 p0 = hexagon[0], p1 = hexagon[1];
// create a Pose graph with one equality constraint and one measurement
shared_ptr<Pose2Graph> fg(new Pose2Graph);
fg->addHardConstraint(0, p0);
shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph);
fg->addPoseConstraint(0, p0);
Pose2 delta = p0.between(p1);
fg->addConstraint(0, 1, delta, covariance);
fg->addConstraint(1,2, delta, covariance);
fg->addConstraint(2,3, delta, covariance);
fg->addConstraint(3,4, delta, covariance);
fg->addConstraint(4,5, delta, covariance);
fg->addConstraint(5, 0, delta, covariance);
fg->addOdometry(0, 1, delta, covariance);
fg->addOdometry(1,2, delta, covariance);
fg->addOdometry(2,3, delta, covariance);
fg->addOdometry(3,4, delta, covariance);
fg->addOdometry(4,5, delta, covariance);
fg->addOdometry(5, 0, delta, covariance);
// Create initial config
boost::shared_ptr<Pose2Values> initial(new Pose2Values());
boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values());
initial->insert(0, p0);
initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1)));
@ -245,7 +247,7 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) {
pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params);
pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
Pose2Values actual = *optimizer.values();
pose2SLAM::Values actual = *optimizer.values();
// Check with ground truth
CHECK(assert_equal(hexagon, actual));
@ -279,7 +281,7 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) {
//
// myOptimizer.update(x);
//
// Pose2Values expected;
// pose2SLAM::Values expected;
// expected.insert(0, Pose2(0.,0.,0.));
// expected.insert(1, Pose2(1.,0.,0.));
// expected.insert(2, Pose2(2.,0.,0.));
@ -307,8 +309,8 @@ TEST(Pose2Graph, optimize2) {
}
///* ************************************************************************* */
// SL-NEEDED? TEST(Pose2Graph, findMinimumSpanningTree) {
// Pose2Graph G, T, C;
// SL-NEEDED? TEST(Pose2SLAM, findMinimumSpanningTree) {
// pose2SLAM::Graph G, T, C;
// G.addConstraint(1, 2, Pose2(0.,0.,0.), I3);
// G.addConstraint(1, 3, Pose2(0.,0.,0.), I3);
// G.addConstraint(2, 3, Pose2(0.,0.,0.), I3);
@ -321,8 +323,8 @@ TEST(Pose2Graph, optimize2) {
//}
//
///* ************************************************************************* */
// SL-NEEDED? TEST(Pose2Graph, split) {
// Pose2Graph G, T, C;
// SL-NEEDED? TEST(Pose2SLAM, split) {
// pose2SLAM::Graph G, T, C;
// G.addConstraint(1, 2, Pose2(0.,0.,0.), I3);
// G.addConstraint(1, 3, Pose2(0.,0.,0.), I3);
// G.addConstraint(2, 3, Pose2(0.,0.,0.), I3);
@ -340,38 +342,38 @@ TEST(Pose2Graph, optimize2) {
using namespace pose2SLAM;
/* ************************************************************************* */
TEST( Pose2Values, pose2Circle )
TEST(Pose2Values, pose2Circle )
{
// expected is 4 poses tangent to circle with radius 1m
Pose2Values expected;
pose2SLAM::Values expected;
expected.insert(0, Pose2( 1, 0, M_PI_2));
expected.insert(1, Pose2( 0, 1, - M_PI ));
expected.insert(2, Pose2(-1, 0, - M_PI_2));
expected.insert(3, Pose2( 0, -1, 0 ));
Pose2Values actual = pose2SLAM::circle(4,1.0);
pose2SLAM::Values actual = pose2SLAM::circle(4,1.0);
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( Pose2Values, expmap )
TEST(Pose2SLAM, expmap )
{
// expected is circle shifted to right
Pose2Values expected;
pose2SLAM::Values expected;
expected.insert(0, Pose2( 1.1, 0, M_PI_2));
expected.insert(1, Pose2( 0.1, 1, - M_PI ));
expected.insert(2, Pose2(-0.9, 0, - M_PI_2));
expected.insert(3, Pose2( 0.1, -1, 0 ));
// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
Pose2Values circle(pose2SLAM::circle(4,1.0));
pose2SLAM::Values circle(pose2SLAM::circle(4,1.0));
Ordering ordering(*circle.orderingArbitrary());
VectorValues delta(circle.dims(ordering));
delta[ordering[Key(0)]] = Vector_(3, 0.0,-0.1,0.0);
delta[ordering[Key(1)]] = Vector_(3, -0.1,0.0,0.0);
delta[ordering[Key(2)]] = Vector_(3, 0.0,0.1,0.0);
delta[ordering[Key(3)]] = Vector_(3, 0.1,0.0,0.0);
Pose2Values actual = circle.retract(delta, ordering);
delta[ordering[PoseKey(0)]] = Vector_(3, 0.0,-0.1,0.0);
delta[ordering[PoseKey(1)]] = Vector_(3, -0.1,0.0,0.0);
delta[ordering[PoseKey(2)]] = Vector_(3, 0.0,0.1,0.0);
delta[ordering[PoseKey(3)]] = Vector_(3, 0.1,0.0,0.0);
pose2SLAM::Values actual = circle.retract(delta, ordering);
CHECK(assert_equal(expected,actual));
}
@ -384,11 +386,11 @@ TEST( Pose2Prior, error )
{
// Choose a linearization point
Pose2 p1(1, 0, 0); // robot at (1,0)
Pose2Values x0;
pose2SLAM::Values x0;
x0.insert(1, p1);
// Create factor
Pose2Prior factor(1, p1, sigmas);
pose2SLAM::Prior factor(1, p1, sigmas);
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
@ -406,7 +408,7 @@ TEST( Pose2Prior, error )
VectorValues addition(VectorValues::Zero(x0.dims(ordering)));
addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0);
VectorValues plus = delta + addition;
Pose2Values x1 = x0.retract(plus, ordering);
pose2SLAM::Values x1 = x0.retract(plus, ordering);
Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
CHECK(assert_equal(error_at_plus,factor.whitenedError(x1)));
CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
@ -415,7 +417,7 @@ TEST( Pose2Prior, error )
/* ************************************************************************* */
// common Pose2Prior for tests below
static gtsam::Pose2 priorVal(2,2,M_PI_2);
static Pose2Prior priorFactor(1,priorVal, sigmas);
static pose2SLAM::Prior priorFactor(1,priorVal, sigmas);
/* ************************************************************************* */
// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
@ -428,7 +430,7 @@ LieVector hprior(const Pose2& p1) {
TEST( Pose2Prior, linearize )
{
// Choose a linearization point at ground truth
Pose2Values x0;
pose2SLAM::Values x0;
x0.insert(1,priorVal);
// Actual linearization
@ -448,7 +450,7 @@ TEST( Pose2Factor, error )
// Choose a linearization point
Pose2 p1; // robot at origin
Pose2 p2(1, 0, 0); // robot at (1,0)
Pose2Values x0;
pose2SLAM::Values x0;
x0.insert(1, p1);
x0.insert(2, p2);
@ -472,7 +474,7 @@ TEST( Pose2Factor, error )
// Check error after increasing p2
VectorValues plus = delta;
plus[ordering["x2"]] = Vector_(3, 0.1, 0.0, 0.0);
Pose2Values x1 = x0.retract(plus, ordering);
pose2SLAM::Values x1 = x0.retract(plus, ordering);
Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
CHECK(assert_equal(error_at_plus,factor.whitenedError(x1)));
CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
@ -489,7 +491,7 @@ TEST( Pose2Factor, rhs )
// Choose a linearization point
Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4)
Pose2Values x0;
pose2SLAM::Values x0;
x0.insert(1,p1);
x0.insert(2,p2);
@ -519,7 +521,7 @@ TEST( Pose2Factor, linearize )
// Choose a linearization point at ground truth
Pose2 p1(1,2,M_PI_2);
Pose2 p2(-1,4,M_PI);
Pose2Values x0;
pose2SLAM::Values x0;
x0.insert(1,p1);
x0.insert(2,p2);

4
tests/testGaussianJunctionTree.cpp
View File

@ -195,8 +195,8 @@ TEST(GaussianJunctionTree, simpleMarginal) {
// Create a simple graph
pose2SLAM::Graph fg;
fg.addPrior(pose2SLAM::Key(0), Pose2(), sharedSigma(3, 10.0));
fg.addConstraint(0, 1, Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0)));
fg.addPrior(pose2SLAM::PoseKey(0), Pose2(), sharedSigma(3, 10.0));
fg.addOdometry(0, 1, Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0)));
pose2SLAM::Values init;
init.insert(0, Pose2());

16
tests/testGraph.cpp
View File

@ -78,15 +78,15 @@ TEST( Graph, predecessorMap2Graph )
/* ************************************************************************* */
TEST( Graph, composePoses )
{
Pose2Graph graph;
pose2SLAM::Graph graph;
Matrix cov = eye(3);
Pose2 p1(1.0, 2.0, 0.3), p2(4.0, 5.0, 0.6), p3(7.0, 8.0, 0.9), p4(2.0, 2.0, 2.9);
Pose2 p12=p1.between(p2), p23=p2.between(p3), p43=p4.between(p3);
graph.addConstraint(1,2, p12, cov);
graph.addConstraint(2,3, p23, cov);
graph.addConstraint(4,3, p43, cov);
graph.addOdometry(1,2, p12, cov);
graph.addOdometry(2,3, p23, cov);
graph.addOdometry(4,3, p43, cov);
PredecessorMap<Pose2Values::Key> tree;
PredecessorMap<pose2SLAM::Values::Key> tree;
tree.insert(1,2);
tree.insert(2,2);
tree.insert(3,2);
@ -94,10 +94,10 @@ TEST( Graph, composePoses )
Pose2 rootPose = p2;
boost::shared_ptr<Pose2Values> actual = composePoses<Pose2Graph, Pose2Factor,
Pose2, Pose2Values> (graph, tree, rootPose);
boost::shared_ptr<pose2SLAM::Values> actual = composePoses<pose2SLAM::Graph, pose2SLAM::Odometry,
Pose2, pose2SLAM::Values> (graph, tree, rootPose);
Pose2Values expected;
pose2SLAM::Values expected;
expected.insert(1, p1);
expected.insert(2, p2);
expected.insert(3, p3);

14
tests/testNonlinearOptimizer.cpp
View File

@ -253,24 +253,24 @@ TEST( NonlinearOptimizer, optimization_method )
/* ************************************************************************* */
TEST( NonlinearOptimizer, Factorization )
{
typedef NonlinearOptimizer<Pose2Graph, Pose2Values, GaussianFactorGraph, GaussianSequentialSolver > Optimizer;
typedef NonlinearOptimizer<pose2SLAM::Graph, pose2SLAM::Values, GaussianFactorGraph, GaussianSequentialSolver > Optimizer;
boost::shared_ptr<Pose2Values> config(new Pose2Values);
boost::shared_ptr<pose2SLAM::Values> config(new pose2SLAM::Values);
config->insert(1, Pose2(0.,0.,0.));
config->insert(2, Pose2(1.5,0.,0.));
boost::shared_ptr<Pose2Graph> graph(new Pose2Graph);
boost::shared_ptr<pose2SLAM::Graph> graph(new pose2SLAM::Graph);
graph->addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
graph->addConstraint(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
graph->addOdometry(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
boost::shared_ptr<Ordering> ordering(new Ordering);
ordering->push_back(Pose2Values::Key(1));
ordering->push_back(Pose2Values::Key(2));
ordering->push_back(pose2SLAM::PoseKey(1));
ordering->push_back(pose2SLAM::PoseKey(2));
Optimizer optimizer(graph, config, ordering);
Optimizer optimized = optimizer.iterateLM();
Pose2Values expected;
pose2SLAM::Values expected;
expected.insert(1, Pose2(0.,0.,0.));
expected.insert(2, Pose2(1.,0.,0.));
CHECK(assert_equal(expected, *optimized.values(), 1e-5));

4
tests/timeMultifrontalOnDataset.cpp
View File

@ -44,13 +44,13 @@ int main(int argc, char *argv[]) {
else if (argc == 4)
nrTrials = strtoul(argv[3], NULL, 10);
pair<shared_ptr<Pose2Graph>, shared_ptr<Pose2Values> > data = load2D(dataset(datasetname));
pair<shared_ptr<pose2SLAM::Graph>, shared_ptr<pose2SLAM::Values> > data = load2D(dataset(datasetname));
// Add a prior on the first pose
if (soft_prior)
data.first->addPrior(0, Pose2(), sharedSigma(Pose2::Dim(), 0.0005));
else
data.first->addHardConstraint(0, Pose2());
data.first->addPoseConstraint(0, Pose2());
tic_(1, "order");
Ordering::shared_ptr ordering(data.first->orderingCOLAMD(*data.second));

4
tests/timeSequentialOnDataset.cpp
View File

@ -44,13 +44,13 @@ int main(int argc, char *argv[]) {
else if (argc == 4)
nrTrials = strtoul(argv[3], NULL, 10);
pair<shared_ptr<Pose2Graph>, shared_ptr<Pose2Values> > data = load2D(dataset(datasetname));
pair<shared_ptr<pose2SLAM::Graph>, shared_ptr<pose2SLAM::Values> > data = load2D(dataset(datasetname));
// Add a prior on the first pose
if (soft_prior)
data.first->addPrior(0, Pose2(), sharedSigma(Pose2::Dim(), 0.0005));
else
data.first->addHardConstraint(0, Pose2());
data.first->addPoseConstraint(0, Pose2());
tic_(1, "order");
Ordering::shared_ptr ordering(data.first->orderingCOLAMD(*data.second));