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

Remove namespace files

release/4.3a0
Stephen Williams 2012-07-30 15:38:58 +00:00
parent 094d395238
commit 15cfa7e702
15 changed files with 0 additions and 2630 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
gtsam/slam/planarSLAM.cpp
View File

@ -1,63 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file planarSLAM.cpp
* @brief: bearing/range measurements in 2D plane
* @author Frank Dellaert
**/
#include <gtsam/slam/planarSLAM.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
// Use planarSLAM namespace for specific SLAM instance
namespace planarSLAM {
/* ************************************************************************* */
Matrix Values::points() const {
size_t j=0;
ConstFiltered<Point2> points = filter<Point2>();
Matrix result(points.size(),2);
BOOST_FOREACH(const ConstFiltered<Point2>::KeyValuePair& keyValue, points)
result.row(j++) = keyValue.value.vector();
return result;
}
/* ************************************************************************* */
void Graph::addPointConstraint(Key pointKey, const Point2& p) {
push_back(boost::make_shared<NonlinearEquality<Point2> >(pointKey, p));
}
/* ************************************************************************* */
void Graph::addPointPrior(Key pointKey, const Point2& p, const SharedNoiseModel& model) {
push_back(boost::make_shared<PriorFactor<Point2> >(pointKey, p, model));
}
/* ************************************************************************* */
void Graph::addBearing(Key i, Key j, const Rot2& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<BearingFactor<Pose2, Point2> >(i, j, z, model));
}
/* ************************************************************************* */
void Graph::addRange(Key i, Key j, double z, const SharedNoiseModel& model) {
push_back(boost::make_shared<RangeFactor<Pose2, Point2> >(i, j, z, model));
}
/* ************************************************************************* */
void Graph::addBearingRange(Key i, Key j, const Rot2& z1,
double z2, const SharedNoiseModel& model) {
push_back(boost::make_shared<BearingRangeFactor<Pose2, Point2> >(i, j, z1, z2, model));
}
/* ************************************************************************* */
} // planarSLAM

122
gtsam/slam/planarSLAM.h
View File

@ -1,122 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file planarSLAM.h
* @brief bearing/range measurements in 2D plane
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/BearingFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/geometry/Pose2.h>
// Use planarSLAM namespace for specific SLAM instance
namespace planarSLAM {
using namespace gtsam;
/*
* Values class, inherited from pose2SLAM::Values, mainly used as a convenience for MATLAB wrapper
* @addtogroup SLAM
*/
struct Values: public pose2SLAM::Values {
typedef boost::shared_ptr<Values> shared_ptr;
typedef gtsam::Values::ConstFiltered<Pose2> PoseFiltered;
typedef gtsam::Values::ConstFiltered<Point2> PointFiltered;
/// Default constructor
Values() {}
/// Copy constructor
Values(const gtsam::Values& values) :
pose2SLAM::Values(values) {
}
/// Constructor from filtered values view of poses
Values(const PoseFiltered& view) : pose2SLAM::Values(view) {}
/// Constructor from filtered values view of points
Values(const PointFiltered& view) : pose2SLAM::Values(view) {}
PoseFiltered allPoses() const { return this->filter<Pose2>(); } ///< pose view
size_t nrPoses() const { return allPoses().size(); } ///< get number of poses
KeyList poseKeys() const { return allPoses().keys(); } ///< get keys to poses only
PointFiltered allPoints() const { return this->filter<Point2>(); } ///< point view
size_t nrPoints() const { return allPoints().size(); } ///< get number of points
KeyList pointKeys() const { return allPoints().keys(); } ///< get keys to points only
/// insert a point
void insertPoint(Key j, const Point2& point) { insert(j, point); }
/// update a point
void updatePoint(Key j, const Point2& point) { update(j, point); }
/// get a point
Point2 point(Key j) const { return at<Point2>(j); }
/// get all [x,y] coordinates in a 2*n matrix
Matrix points() const;
};
/**
* Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper
* @addtogroup SLAM
*/
struct Graph: public pose2SLAM::Graph {
/// Default constructor
Graph(){}
/// Copy constructor given any other NonlinearFactorGraph
Graph(const NonlinearFactorGraph& graph):
pose2SLAM::Graph(graph) {}
/// Creates a hard constraint on a point
void addPointConstraint(Key j, const Point2& p);
/// Adds a prior with mean p and given noise model on point j
void addPointPrior(Key j, const Point2& p, const SharedNoiseModel& model);
/// Creates a bearing measurement from pose i to point j
void addBearing(Key i, Key j, const Rot2& bearing, const SharedNoiseModel& model);
/// Creates a range measurement from pose i to point j
void addRange(Key i, Key k, double range, const SharedNoiseModel& model);
/// Creates a range/bearing measurement from pose i to point j
void addBearingRange(Key i, Key j, const Rot2& bearing, double range, const SharedNoiseModel& model);
};
} // planarSLAM
/**
* Backwards compatibility and wrap use only, avoid using
*/
namespace planarSLAM {
typedef gtsam::NonlinearEquality<Pose2> Constraint; ///< \deprecated typedef for backwards compatibility
typedef gtsam::PriorFactor<Pose2> Prior; ///< \deprecated typedef for backwards compatibility
typedef gtsam::BetweenFactor<Pose2> Odometry; ///< \deprecated typedef for backwards compatibility
typedef gtsam::BearingFactor<Pose2, Point2> Bearing; ///< \deprecated typedef for backwards compatibility
typedef gtsam::RangeFactor<Pose2, Point2> Range; ///< \deprecated typedef for backwards compatibility
typedef gtsam::BearingRangeFactor<Pose2, Point2> BearingRange; ///< \deprecated typedef for backwards compatibility
}

66
gtsam/slam/pose2SLAM.cpp
View File

@ -1,66 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file pose2SLAM.cpp
* @brief: Odometry measurements in 2D plane
* @author Frank Dellaert
**/
#include <gtsam/slam/pose2SLAM.h>
// Use pose2SLAM namespace for specific SLAM instance
namespace pose2SLAM {
/* ************************************************************************* */
Values Values::Circle(size_t n, double R) {
Values x;
double theta = 0, dtheta = 2 * M_PI / n;
for (size_t i = 0; i < n; i++, theta += dtheta)
x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
return x;
}
/* ************************************************************************* */
Matrix Values::poses() const {
size_t j=0;
ConstFiltered<Pose2> poses = filter<Pose2>();
Matrix result(poses.size(),3);
BOOST_FOREACH(const ConstFiltered<Pose2>::KeyValuePair& keyValue, poses) {
const Pose2& r = keyValue.value;
result.row(j++) = Matrix_(1,3, r.x(), r.y(), r.theta());
}
return result;
}
/* ************************************************************************* */
void Graph::addPoseConstraint(Key i, const Pose2& p) {
sharedFactor factor(new NonlinearEquality<Pose2>(i, p));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPosePrior(Key i, const Pose2& p, const SharedNoiseModel& model) {
sharedFactor factor(new PriorFactor<Pose2>(i, p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addRelativePose(Key i1, Key i2, const Pose2& z,
const SharedNoiseModel& model) {
sharedFactor factor(new BetweenFactor<Pose2>(i1, i2, z, model));
push_back(factor);
}
/* ************************************************************************* */
} // pose2SLAM

107
gtsam/slam/pose2SLAM.h
View File

@ -1,107 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file pose2SLAM.h
* @brief: 2D Pose SLAM
* @author Frank Dellaert
**/
#pragma once
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/EasyFactorGraph.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/geometry/Pose2.h>
// Use pose2SLAM namespace for specific SLAM instance
namespace pose2SLAM {
using namespace gtsam;
/*
* Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
* @addtogroup SLAM
*/
struct Values: public gtsam::Values {
typedef boost::shared_ptr<Values> shared_ptr;
/// Default constructor
Values() {}
/// Copy constructor
Values(const gtsam::Values& values) :
gtsam::Values(values) {
}
/**
* 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
*/
static Values Circle(size_t n, double R);
/// insert a pose
void insertPose(Key i, const Pose2& pose) { insert(i, pose); }
/// update a pose
void updatePose(Key i, const Pose2& pose) { update(i, pose); }
/// get a pose
Pose2 pose(Key i) const { return at<Pose2>(i); }
/// get all [x,y,theta] coordinates in a 3*m matrix
Matrix poses() const;
};
/**
* List of typedefs for factors
*/
/**
* Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper
* @addtogroup SLAM
*/
struct Graph: public EasyFactorGraph {
typedef boost::shared_ptr<Graph> shared_ptr;
/// Default constructor
Graph(){}
/// Copy constructor given any other NonlinearFactorGraph
Graph(const NonlinearFactorGraph& graph):
EasyFactorGraph(graph) {}
/// Creates a hard constraint for key i with the given Pose2 p.
void addPoseConstraint(Key i, const Pose2& p);
/// Adds a Pose2 prior with mean p and given noise model on pose i
void addPosePrior(Key i, const Pose2& p, const SharedNoiseModel& model);
/// Creates an relative pose factor between poses with keys i1 and i2
void addRelativePose(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model);
};
} // pose2SLAM
/**
* Backwards compatibility and wrap use only, avoid using
*/
namespace pose2SLAM {
typedef gtsam::NonlinearEquality<Pose2> HardConstraint; ///< \deprecated typedef for backwards compatibility
typedef gtsam::PriorFactor<Pose2> Prior; ///< \deprecated typedef for backwards compatibility
typedef gtsam::BetweenFactor<Pose2> Odometry; ///< \deprecated typedef for backwards compatibility
}

73
gtsam/slam/pose3SLAM.cpp
View File

@ -1,73 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file pose3SLAM.cpp
* @brief: bearing/range measurements in 2D plane
* @author Frank Dellaert
**/
#include <gtsam/slam/pose3SLAM.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// Use pose3SLAM namespace for specific SLAM instance
namespace pose3SLAM {
/* ************************************************************************* */
Values Values::Circle(size_t n, double radius) {
Values x;
double theta = 0, dtheta = 2 * M_PI / n;
// We use aerospace/navlab convention, X forward, Y right, Z down
// First pose will be at (R,0,0)
// ^y ^ X
// | |
// z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer)
// Vehicle at p0 is looking towards y axis (X-axis points towards world y)
Rot3 gR0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
for (size_t i = 0; i < n; i++, theta += dtheta) {
Point3 gti(radius*cos(theta), radius*sin(theta), 0);
Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down !
Pose3 gTi(gR0 * _0Ri, gti);
x.insert(i, gTi);
}
return x;
}
/* ************************************************************************* */
Matrix Values::translations() const {
size_t j=0;
ConstFiltered<Pose3> poses = filter<Pose3>();
Matrix result(poses.size(),3);
BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
result.row(j++) = keyValue.value.translation().vector();
return result;
}
/* ************************************************************************* */
void Graph::addPoseConstraint(Key i, const Pose3& p) {
sharedFactor factor(new NonlinearEquality<Pose3>(i, p));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPosePrior(Key i, const Pose3& p, const SharedNoiseModel& model) {
sharedFactor factor(new PriorFactor<Pose3>(i, p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addRelativePose(Key i1, Key i2, const Pose3& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<BetweenFactor<Pose3> >(i1, i2, z, model));
}
/* ************************************************************************* */
} // pose3SLAM

120
gtsam/slam/pose3SLAM.h
View File

@ -1,120 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file pose3SLAM.h
* @brief: 3D Pose SLAM
* @author Frank Dellaert
**/
#pragma once
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/EasyFactorGraph.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/geometry/Pose3.h>
/// Use pose3SLAM namespace for specific SLAM instance
namespace pose3SLAM {
using namespace gtsam;
/*
* Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
* @addtogroup SLAM
*/
struct Values: public gtsam::Values {
typedef boost::shared_ptr<Values> shared_ptr;
/// Default constructor
Values() {}
/// Copy constructor
Values(const gtsam::Values& values) :
gtsam::Values(values) {
}
/**
* Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)
* @param n number of poses
* @param R radius of circle
* @return circle of n 3D poses
*/
static Values Circle(size_t n, double R);
/// insert a pose
void insertPose(Key i, const Pose3& pose) { insert(i, pose); }
/// update a pose
void updatePose(Key i, const Pose3& pose) { update(i, pose); }
/// get a pose
Pose3 pose(Key i) const { return at<Pose3>(i); }
Matrix translations() const; ///< get all pose translations coordinates in a matrix
};
/**
* Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper
* @addtogroup SLAM
*/
struct Graph: public EasyFactorGraph {
/// Default constructor
Graph(){}
/// Copy constructor given any other NonlinearFactorGraph
Graph(const NonlinearFactorGraph& graph):
EasyFactorGraph(graph) {}
/**
* Add a prior on a pose
* @param key variable key of the camera pose
* @param p around which soft prior is defined
* @param model uncertainty model of this prior
*/
void addPosePrior(Key poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6));
/**
* Add a constraint on a pose (for now, *must* be satisfied in any Values)
* @param key variable key of the camera pose
* @param p to which pose to constrain it to
*/
void addPoseConstraint(Key poseKey, const Pose3& p = Pose3());
/**
* Add a relative pose measurement between two poses
* @param x1 variable key of the first camera pose
* @param x2 variable key of the second camera pose
* @param relative pose measurement from x1 to x2 (x1.between(x2))
* @param model uncertainty model of this measurement
*/
void addRelativePose(Key x1, Key x2, const Pose3& z, const SharedNoiseModel& model);
};
} // pose3SLAM
/**
* Backwards compatibility and wrap use only, avoid using
*/
namespace pose3SLAM {
typedef gtsam::PriorFactor<Pose3> Prior; ///< \deprecated typedef for backwards compatibility
typedef gtsam::BetweenFactor<Pose3> Constraint; ///< \deprecated typedef for backwards compatibility
typedef gtsam::NonlinearEquality<Pose3> HardConstraint; ///< \deprecated typedef for backwards compatibility
}
namespace gtsam {
typedef pose3SLAM::Prior Pose3Prior; ///< \deprecated typedef for backwards compatibility
typedef pose3SLAM::Constraint Pose3Factor; ///< \deprecated typedef for backwards compatibility
typedef pose3SLAM::Graph Pose3Graph; ///< \deprecated typedef for backwards compatibility
}

60
gtsam/slam/sparseBA.cpp
View File

@ -1,60 +0,0 @@
/**
* @file sparseBA.cpp
* @brief
* @date Jul 6, 2012
* @author Yong-Dian Jian
*/
#include <gtsam/slam/sparseBA.h>
namespace sparseBA {
/* ************************************************************************* */
void Graph::addSimpleCameraConstraint(Key cameraKey, const SimpleCamera &camera) {
addCameraConstraint<SimpleCamera>(cameraKey, camera);
}
/* ************************************************************************* */
void Graph::addSimpleCameraPrior(Key cameraKey, const SimpleCamera &camera, SharedNoiseModel &model) {
addCameraPrior<SimpleCamera>(cameraKey, camera, model);
}
/* ************************************************************************* */
void Graph::addSimpleCameraMeasurement(const Point2 &z, SharedNoiseModel& model, Index cameraKey, Index pointKey) {
addMeasurement<SimpleCamera>(z, model, cameraKey, pointKey);
}
/* ************************************************************************* */
Matrix Graph::reprojectionErrors(const Values& values) const {
// TODO: support the other calibration objects. Now it only works for Cal3_S2.
typedef GeneralSFMFactor<SimpleCamera, Point3> SFMFactor;
typedef GeneralSFMFactor2<Cal3_S2> SFMFactor2;
// first count
size_t K = 0, k=0;
BOOST_FOREACH(const sharedFactor& f, *this)
if (boost::dynamic_pointer_cast<const SFMFactor>(f)) ++K;
else if (boost::dynamic_pointer_cast<const SFMFactor2>(f)) ++K;
// now fill
Matrix errors(2,K);
BOOST_FOREACH(const sharedFactor& f, *this) {
boost::shared_ptr<const SFMFactor> p = boost::dynamic_pointer_cast<const SFMFactor>(f);
if (p) {
errors.col(k++) = p->unwhitenedError(values);
continue;
}
boost::shared_ptr<const SFMFactor2> p2 = boost::dynamic_pointer_cast<const SFMFactor2>(f);
if (p2) {
errors.col(k++) = p2->unwhitenedError(values);
}
}
return errors;
}
/* ************************************************************************* */
}

143
gtsam/slam/sparseBA.h
View File

@ -1,143 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file sba.h
* @brief a suite for sparse bundle adjustment
* @date Jul 5, 2012
* @author ydjian
*/
#pragma once
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/geometry/SimpleCamera.h>
namespace sparseBA {
using namespace gtsam;
/// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
struct Values: public visualSLAM::Values {
typedef boost::shared_ptr<Values> shared_ptr;
typedef gtsam::Values::ConstFiltered<SimpleCamera> SimpleCameraFiltered;
typedef gtsam::Values::ConstFiltered<Cal3_S2> Cal3_S2Filtered;
/// Default constructor
Values() {}
/// Copy constructor
Values(const gtsam::Values& values) : visualSLAM::Values(values) {}
/// Constructor from filtered values view of poses
Values(const SimpleCameraFiltered& view) : visualSLAM::Values(view) {}
/// Constructor from filtered values view of points
Values(const PointFiltered& view) : visualSLAM::Values(view) {}
SimpleCameraFiltered allSimpleCameras() const { return this->filter<SimpleCamera>(); } ///< camera view
size_t nrSimpleCameras() const { return allSimpleCameras().size(); } ///< get number of poses
KeyList simpleCameraKeys() const { return allSimpleCameras().keys(); } ///< get keys to poses only
/// insert a camera
void insertSimpleCamera(Key j, const SimpleCamera& camera) { insert(j, camera); }
/// update a camera
void updateSimpleCamera(Key j, const SimpleCamera& camera) { update(j, camera); }
/// get a camera
SimpleCamera simpleCamera(Key j) const { return at<SimpleCamera>(j); }
};
/**
* Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper
* @addtogroup SLAM
*/
struct Graph: public visualSLAM::Graph {
/// Default constructor
Graph(){}
/// Copy constructor given any other NonlinearFactorGraph
Graph(const NonlinearFactorGraph& graph): visualSLAM::Graph(graph) {}
/// check if two graphs are equal
bool equals(const Graph& p, double tol = 1e-9) const {
return NonlinearFactorGraph::equals(p, tol);
}
/**
* Add a prior on a pose
* @param key variable key of the camera
* @param p around which soft prior is defined
* @param model uncertainty model of this prior
*/
template <typename Camera>
void addCameraPrior(Key cameraKey, const Camera &camera, SharedNoiseModel &model = noiseModel::Unit::Create(Camera::Dim())) {
sharedFactor factor(new PriorFactor<Camera>(cameraKey, camera, model));
push_back(factor);
}
/**
* Add a constraint on a camera
* @param key variable key of the camera
* @param p to which camera to constrain it to
*/
template <typename Camera>
void addCameraConstraint(Key cameraKey, const Camera &camera) {
sharedFactor factor(new NonlinearEquality<Camera>(cameraKey, camera));
push_back(factor);
}
/**
* Add a 2d projection measurement
* @param z the measurement
* @param model the noise model for the measurement
* @param cameraKey variable key for the pose+calibration
* @param pointKey variable key for the point
*/
template <typename Camera>
void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index cameraKey, const Index pointKey) {
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
boost::shared_ptr<SFMFactor> factor(new SFMFactor(z, model, cameraKey, pointKey));
push_back(factor);
}
/**
* Add a 2d projection measurement, but supports separated (or shared) pose and calibration object
* @param z the measurement
* @param model the noise model for the measurement
* @param poseKey variable key for the pose
* @param pointKey variable key for the point
* @param calibKey variable key for the calibration
*/
template <typename Calib>
void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index posekey, const Index pointkey, const Index calibkey) {
typedef GeneralSFMFactor2<Calib> SFMFactor;
boost::shared_ptr<SFMFactor> factor(new SFMFactor(z, model, posekey, pointkey, calibkey));
push_back(factor);
}
/// Return a 2*K Matrix of reprojection errors
Matrix reprojectionErrors(const Values& values) const;
/**
* Matlab-specific wrappers
*/
void addSimpleCameraPrior(Key cameraKey, const SimpleCamera &camera, SharedNoiseModel &model);
void addSimpleCameraConstraint(Key cameraKey, const SimpleCamera &camera) ;
void addSimpleCameraMeasurement(const Point2 &z, SharedNoiseModel& model, Index cameraKey, Index pointKey);
};
}

218
gtsam/slam/tests/testPlanarSLAM.cpp
View File

@ -1,218 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testPlanarSLAM.cpp
* @author Frank Dellaert
**/
#include <gtsam/slam/planarSLAM.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/base/TestableAssertions.h>
#include <iostream>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
// some shared test values
static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
static Symbol i2('x',2), i3('x',3), j3('l',3);
SharedNoiseModel
sigma(noiseModel::Isotropic::Sigma(1,0.1)),
sigma2(noiseModel::Isotropic::Sigma(2,0.1)),
I3(noiseModel::Unit::Create(3));
/* ************************************************************************* */
TEST( planarSLAM, PriorFactor_equals )
{
PriorFactor<Pose2> factor1(i2, x1, I3), factor2(i2, x2, I3);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));
}
/* ************************************************************************* */
TEST( planarSLAM, BearingFactor )
{
// Create factor
Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
BearingFactor<Pose2, Point2> factor(i2, j3, z, sigma);
// create config
planarSLAM::Values c;
c.insert(i2, x2);
c.insert(j3, l3);
// Check error
Vector actual = factor.unwhitenedError(c);
EXPECT(assert_equal(Vector_(1,-0.1),actual));
}
/* ************************************************************************* */
TEST( planarSLAM, BearingFactor_equals )
{
BearingFactor<Pose2, Point2>
factor1(i2, j3, Rot2::fromAngle(0.1), sigma),
factor2(i2, j3, Rot2::fromAngle(2.3), sigma);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));
}
/* ************************************************************************* */
TEST( planarSLAM, RangeFactor )
{
// Create factor
double z(sqrt(2.0) - 0.22); // h(x) - z = 0.22
RangeFactor<Pose2, Point2> factor(i2, j3, z, sigma);
// create config
planarSLAM::Values c;
c.insert(i2, x2);
c.insert(j3, l3);
// Check error
Vector actual = factor.unwhitenedError(c);
EXPECT(assert_equal(Vector_(1,0.22),actual));
}
/* ************************************************************************* */
TEST( planarSLAM, RangeFactor_equals )
{
RangeFactor<Pose2, Point2> factor1(i2, j3, 1.2, sigma), factor2(i2, j3, 7.2, sigma);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));
}
/* ************************************************************************* */
TEST( planarSLAM, BearingRangeFactor )
{
// Create factor
Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
double b(sqrt(2.0) - 0.22); // h(x) - z = 0.22
BearingRangeFactor<Pose2, Point2> factor(i2, j3, r, b, sigma2);
// create config
planarSLAM::Values c;
c.insert(i2, x2);
c.insert(j3, l3);
// Check error
Vector actual = factor.unwhitenedError(c);
EXPECT(assert_equal(Vector_(2,-0.1, 0.22),actual));
}
/* ************************************************************************* */
TEST( planarSLAM, BearingRangeFactor_equals )
{
BearingRangeFactor<Pose2, Point2>
factor1(i2, j3, Rot2::fromAngle(0.1), 7.3, sigma2),
factor2(i2, j3, Rot2::fromAngle(3), 2.0, sigma2);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));
}
/* ************************************************************************* */
TEST( planarSLAM, BearingRangeFactor_poses )
{
typedef BearingRangeFactor<Pose2,Pose2> PoseBearingRange;
PoseBearingRange actual(2, 3, Rot2::fromDegrees(60.0), 12.3, sigma2);
}
/* ************************************************************************* */
TEST( planarSLAM, PoseConstraint_equals )
{
NonlinearEquality<Pose2> factor1(i2, x2), factor2(i2, x3);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));
}
/* ************************************************************************* */
TEST( planarSLAM, constructor )
{
// create config
planarSLAM::Values c;
c.insert(i2, x2);
c.insert(i3, x3);
c.insert(j3, l3);
// create graph
planarSLAM::Graph G;
// Add pose constraint
G.addPoseConstraint(i2, x2); // make it feasible :-)
// Add odometry
G.addRelativePose(i2, i3, Pose2(0, 0, M_PI_4), I3);
// Create bearing factor
Rot2 z1 = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
G.addBearing(i2, j3, z1, sigma);
// Create range factor
double z2(sqrt(2.0) - 0.22); // h(x) - z = 0.22
G.addRange(i2, j3, z2, sigma);
Vector expected0 = Vector_(3, 0.0, 0.0, 0.0);
Vector expected1 = Vector_(3, 0.0, 0.0, 0.0);
Vector expected2 = Vector_(1, -0.1);
Vector expected3 = Vector_(1, 0.22);
// Get NoiseModelFactors
EXPECT(assert_equal(expected0, boost::dynamic_pointer_cast<NoiseModelFactor>(G[0])->unwhitenedError(c)));
EXPECT(assert_equal(expected1, boost::dynamic_pointer_cast<NoiseModelFactor>(G[1])->unwhitenedError(c)));
EXPECT(assert_equal(expected2, boost::dynamic_pointer_cast<NoiseModelFactor>(G[2])->unwhitenedError(c)));
EXPECT(assert_equal(expected3, boost::dynamic_pointer_cast<NoiseModelFactor>(G[3])->unwhitenedError(c)));
}
/* ************************************************************************* */
TEST( planarSLAM, keys_and_view )
{
// create config
planarSLAM::Values c;
c.insert(i2, x2);
c.insert(i3, x3);
c.insert(j3, l3);
LONGS_EQUAL(2,c.nrPoses());
LONGS_EQUAL(1,c.nrPoints());
{
FastList<Key> expected, actual;
expected += j3, i2, i3;
actual = c.keys();
CHECK(expected == actual);
}
{
FastList<Key> expected, actual;
expected += i2, i3;
actual = c.poseKeys();
CHECK(expected == actual);
}
{
FastList<Key> expected, actual;
expected += j3;
actual = c.pointKeys();
CHECK(expected == actual);
}
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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

@ -1,608 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testPose2SLAM.cpp
* @author Frank Dellaert, Viorela Ila
**/
#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;
#include <CppUnitLite/TestHarness.h>
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp>
using namespace boost;
using namespace boost::assign;
#include <iostream>
using namespace std;
typedef BetweenFactor<Pose2> Pose2Factor;
// common measurement covariance
static double sx=0.5, sy=0.5,st=0.1;
static Matrix cov(Matrix_(3, 3,
sx*sx, 0.0, 0.0,
0.0, sy*sy, 0.0,
0.0, 0.0, st*st
));
static noiseModel::Gaussian::shared_ptr covariance(noiseModel::Gaussian::Covariance(cov));
/* ************************************************************************* */
TEST_UNSAFE( Pose2SLAM, XYT )
{
pose2SLAM::Values values;
values.insertPose(1,Pose2(1,2,3));
values.insertPose(2,Pose2(4,5,6));
Matrix expected = Matrix_(2,3, 1.0,2.0,3.0, 4.0,5.0,6.0-2*M_PI);
EXPECT(assert_equal(expected,values.poses()));
}
/* ************************************************************************* */
// Test constraint, small displacement
Vector f1(const Pose2& pose1, const Pose2& pose2) {
Pose2 z(2.1130913087, 0.468461064817, 0.436332312999);
Pose2Factor constraint(1, 2, z, covariance);
return constraint.evaluateError(pose1, pose2);
}
TEST_UNSAFE( Pose2SLAM, constraint1 )
{
// create a factor between unknown poses p1 and p2
Pose2 pose1, pose2(2.1130913087, 0.468461064817, 0.436332312999);
Pose2Factor constraint(1, 2, pose2, covariance);
Matrix H1, H2;
Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
Matrix numericalH1 = numericalDerivative21(&f1 , pose1, pose2);
EXPECT(assert_equal(numericalH1,H1,5e-3));
Matrix numericalH2 = numericalDerivative22(&f1 , pose1, pose2);
EXPECT(assert_equal(numericalH2,H2));
}
/* ************************************************************************* */
// Test constraint, large displacement
Vector f2(const Pose2& pose1, const Pose2& pose2) {
Pose2 z(2,2,M_PI_2);
Pose2Factor constraint(1, 2, z, covariance);
return constraint.evaluateError(pose1, pose2);
}
TEST_UNSAFE( Pose2SLAM, constraint2 )
{
// create a factor between unknown poses p1 and p2
Pose2 pose1, pose2(2,2,M_PI_2);
Pose2Factor constraint(1, 2, pose2, covariance);
Matrix H1, H2;
Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
Matrix numericalH1 = numericalDerivative21(&f2 , pose1, pose2);
EXPECT(assert_equal(numericalH1,H1,5e-3));
Matrix numericalH2 = numericalDerivative22(&f2 , pose1, pose2);
EXPECT(assert_equal(numericalH2,H2));
}
/* ************************************************************************* */
TEST_UNSAFE( Pose2SLAM, constructor )
{
// create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2);
pose2SLAM::Graph graph;
graph.addRelativePose(1,2,measured, covariance);
// get the size of the graph
size_t actual = graph.size();
// verify
size_t expected = 1;
CHECK(actual == expected);
}
/* ************************************************************************* */
TEST_UNSAFE( Pose2SLAM, linearization )
{
// create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2);
Pose2Factor constraint(1, 2, measured, covariance);
pose2SLAM::Graph graph;
graph.addRelativePose(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)
pose2SLAM::Values config;
config.insert(1,p1);
config.insert(2,p2);
// Linearize
Ordering ordering(*config.orderingArbitrary());
boost::shared_ptr<FactorGraph<GaussianFactor> > lfg_linearized = graph.linearize(config, ordering);
//lfg_linearized->print("lfg_actual");
// the expected linear factor
FactorGraph<GaussianFactor> lfg_expected;
Matrix A1 = Matrix_(3,3,
0.0,-2.0, -4.2,
2.0, 0.0, -4.2,
0.0, 0.0,-10.0);
Matrix A2 = Matrix_(3,3,
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 10.0);
Vector b = Vector_(3,-0.1/sx,0.1/sy,0.0);
SharedDiagonal probModel1 = noiseModel::Unit::Create(3);
lfg_expected.push_back(JacobianFactor::shared_ptr(new JacobianFactor(ordering[1], A1, ordering[2], A2, b, probModel1)));
CHECK(assert_equal(lfg_expected, *lfg_linearized));
}
/* ************************************************************************* */
TEST_UNSAFE(Pose2SLAM, optimize) {
// create a Pose graph with one equality constraint and one measurement
pose2SLAM::Graph fg;
fg.addPoseConstraint(0, Pose2(0,0,0));
fg.addRelativePose(0, 1, Pose2(1,2,M_PI_2), covariance);
// Create initial config
Values initial;
initial.insert(0, Pose2(0,0,0));
initial.insert(1, Pose2(0,0,0));
// Choose an ordering and optimize
Ordering ordering;
ordering += 0, 1;
LevenbergMarquardtParams params;
params.relativeErrorTol = 1e-15;
params.ordering = ordering;
Values actual = LevenbergMarquardtOptimizer(fg, initial, params).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 marginals
Marginals marginals = fg.marginals(actual);
// Matrix expectedP0 = Infinity, as we have a pose constraint !?
// Matrix actualP0 = marginals.marginalCovariance(0);
// EQUALITY(expectedP0, actualP0);
Matrix expectedP1 = cov; // the second pose really should have just the noise covariance
Matrix actualP1 = marginals.marginalCovariance(1);
EQUALITY(expectedP1, actualP1);
}
/* ************************************************************************* */
TEST_UNSAFE(Pose2SLAM, optimizeSPCG) {
// create a Pose graph with one equality constraint and one measurement
pose2SLAM::Graph fg;
fg.addPosePrior(0, Pose2(0,0,0), noiseModel::Diagonal::Sigmas(Vector_(3,3.0,3.0,1.0)));
fg.addRelativePose(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.addRelativePose(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,0);
// 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) {
// Create a hexagon of poses
pose2SLAM::Values hexagon = pose2SLAM::Values::Circle(3,1.0);
Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1);
// create a Pose graph with one equality constraint and one measurement
pose2SLAM::Graph fg;
fg.addPoseConstraint(0, p0);
Pose2 delta = p0.between(p1);
fg.addRelativePose(0, 1, delta, covariance);
fg.addRelativePose(1, 2, delta, covariance);
fg.addRelativePose(2, 0, delta, covariance);
// Create initial config
pose2SLAM::Values initial;
initial.insertPose(0, p0);
initial.insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1)));
initial.insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1)));
// Choose an ordering
Ordering ordering;
ordering += 0,1,2;
// optimize
LevenbergMarquardtParams params;
params.relativeErrorTol = 1e-15;
params.ordering = ordering;
Values actual = LevenbergMarquardtOptimizer(fg, initial, params).optimize();
// Check with ground truth
CHECK(assert_equal((const Values&)hexagon, actual));
}
/* ************************************************************************* */
// test optimization with 6 poses arranged in a hexagon and a loop closure
TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
// Create a hexagon of poses
pose2SLAM::Values hexagon = pose2SLAM::Values::Circle(6,1.0);
Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1);
// create a Pose graph with one equality constraint and one measurement
pose2SLAM::Graph fg;
fg.addPoseConstraint(0, p0);
Pose2 delta = p0.between(p1);
fg.addRelativePose(0,1, delta, covariance);
fg.addRelativePose(1,2, delta, covariance);
fg.addRelativePose(2,3, delta, covariance);
fg.addRelativePose(3,4, delta, covariance);
fg.addRelativePose(4,5, delta, covariance);
fg.addRelativePose(5,0, delta, covariance);
// Create initial config
pose2SLAM::Values initial;
initial.insertPose(0, p0);
initial.insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1)));
initial.insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1)));
initial.insertPose(3, hexagon.pose(3).retract(Vector_(3,-0.1, 0.1,-0.1)));
initial.insertPose(4, hexagon.pose(4).retract(Vector_(3, 0.1,-0.1, 0.1)));
initial.insertPose(5, hexagon.pose(5).retract(Vector_(3,-0.1, 0.1,-0.1)));
// Choose an ordering
Ordering ordering;
ordering += 0,1,2,3,4,5;
// optimize
LevenbergMarquardtParams params;
params.relativeErrorTol = 1e-15;
params.ordering = ordering;
Values actual = LevenbergMarquardtOptimizer(fg, initial, params).optimize();
// Check with ground truth
CHECK(assert_equal((const Values&)hexagon, actual));
// Check loop closure
CHECK(assert_equal(delta, actual.at<Pose2>(5).between(actual.at<Pose2>(0))));
// Pose2SLAMOptimizer myOptimizer("3");
// Matrix A1 = myOptimizer.a1();
// LONGS_EQUAL(3, A1.rows());
// LONGS_EQUAL(17, A1.cols()); // 7 + 7 + 3
//
// Matrix A2 = myOptimizer.a2();
// LONGS_EQUAL(3, A1.rows());
// LONGS_EQUAL(7, A2.cols()); // 7
//
// Vector b1 = myOptimizer.b1();
// LONGS_EQUAL(9, b1.size()); // 3 + 3 + 3
//
// Vector b2 = myOptimizer.b2();
// LONGS_EQUAL(3, b2.size()); // 3
//
// // Here, call matlab to
// // A=[A1;A2], b=[b1;b2]
// // R=qr(A1)
// // call pcg on A,b, with preconditioner R -> get x
//
// Vector x = myOptimizer.optimize();
// LONGS_EQUAL(9, x.size()); // 3 + 3 + 3
//
// myOptimizer.update(x);
//
// Values expected;
// expected.insert(0, Pose2(0.,0.,0.));
// expected.insert(1, Pose2(1.,0.,0.));
// expected.insert(2, Pose2(2.,0.,0.));
//
// // Check with ground truth
// CHECK(assert_equal(expected, *myOptimizer.theta()));
}
/* ************************************************************************* */
TEST_UNSAFE(Pose2SLAM, optimize2) {
// Pose2SLAMOptimizer myOptimizer("100");
// Matrix A1 = myOptimizer.a1();
// Matrix A2 = myOptimizer.a2();
// cout << "A1: " << A1.rows() << " " << A1.cols() << endl;
// cout << "A2: " << A2.rows() << " " << A2.cols() << endl;
//
// //cout << "error: " << myOptimizer.error() << endl;
// for(int i = 0; i<10; i++) {
// myOptimizer.linearize();
// Vector x = myOptimizer.optimize();
// myOptimizer.update(x);
// }
// //cout << "error: " << myOptimizer.error() << endl;
// CHECK(myOptimizer.error() < 1.);
}
///* ************************************************************************* */
// SL-NEEDED? TEST_UNSAFE(Pose2SLAM, findMinimumSpanningTree) {
// pose2SLAM::Graph G, T, C;
// G.addPoseConstraint(1, 2, Pose2(0.,0.,0.), I3);
// G.addPoseConstraint(1, 3, Pose2(0.,0.,0.), I3);
// G.addPoseConstraint(2, 3, Pose2(0.,0.,0.), I3);
//
// PredecessorMap<pose2SLAM::pose2SLAM::PoseKey> tree =
// G.findMinimumSpanningTree<pose2SLAM::pose2SLAM::PoseKey, Pose2Factor>();
// CHECK(tree[1] == 1);
// CHECK(tree[2] == 1);
// CHECK(tree[3] == 1);
//}
//
///* ************************************************************************* */
// SL-NEEDED? TEST_UNSAFE(Pose2SLAM, split) {
// pose2SLAM::Graph G, T, C;
// G.addPoseConstraint(1, 2, Pose2(0.,0.,0.), I3);
// G.addPoseConstraint(1, 3, Pose2(0.,0.,0.), I3);
// G.addPoseConstraint(2, 3, Pose2(0.,0.,0.), I3);
//
// PredecessorMap<pose2SLAM::pose2SLAM::PoseKey> tree;
// tree.insert(1,2);
// tree.insert(2,2);
// tree.insert(3,2);
//
// G.split<pose2SLAM::pose2SLAM::PoseKey, Pose2Factor>(tree, T, C);
// LONGS_EQUAL(2, T.size());
// LONGS_EQUAL(1, C.size());
//}
using namespace pose2SLAM;
/* ************************************************************************* */
TEST_UNSAFE(Pose2Values, pose2Circle )
{
// expected is 4 poses tangent to circle with radius 1m
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 ));
pose2SLAM::Values actual = pose2SLAM::Values::Circle(4,1.0);
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST_UNSAFE(Pose2SLAM, expmap )
{
// expected is circle shifted to right
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 !!!
pose2SLAM::Values circle = pose2SLAM::Values::Circle(4,1.0);
Ordering ordering(*circle.orderingArbitrary());
VectorValues delta(circle.dims(ordering));
delta[ordering[0]] = Vector_(3, 0.0,-0.1,0.0);
delta[ordering[1]] = Vector_(3, -0.1,0.0,0.0);
delta[ordering[2]] = Vector_(3, 0.0,0.1,0.0);
delta[ordering[3]] = Vector_(3, 0.1,0.0,0.0);
pose2SLAM::Values actual = circle.retract(delta, ordering);
CHECK(assert_equal(expected,actual));
}
// Common measurement covariance
static SharedNoiseModel sigmas = noiseModel::Diagonal::Sigmas(Vector_(3,sx,sy,st));
/* ************************************************************************* */
// Very simple test establishing Ax-b \approx z-h(x)
TEST_UNSAFE( Pose2Prior, error )
{
// Choose a linearization point
Pose2 p1(1, 0, 0); // robot at (1,0)
pose2SLAM::Values x0;
x0.insert(1, p1);
// Create factor
PriorFactor<Pose2> factor(1, p1, sigmas);
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
boost::shared_ptr<JacobianFactor> linear =
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(x0, ordering));
// Check error at x0, i.e. delta = zero !
VectorValues delta(VectorValues::Zero(x0.dims(ordering)));
delta[ordering[1]] = zero(3);
Vector error_at_zero = Vector_(3,0.0,0.0,0.0);
CHECK(assert_equal(error_at_zero,factor.whitenedError(x0)));
CHECK(assert_equal(-error_at_zero,linear->error_vector(delta)));
// Check error after increasing p2
VectorValues addition(VectorValues::Zero(x0.dims(ordering)));
addition[ordering[1]] = Vector_(3, 0.1, 0.0, 0.0);
VectorValues plus = delta + addition;
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)));
}
/* ************************************************************************* */
// common Pose2Prior for tests below
static gtsam::Pose2 priorVal(2,2,M_PI_2);
static PriorFactor<Pose2> priorFactor(1, priorVal, sigmas);
/* ************************************************************************* */
// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
// Hence i.e., b = approximates z-h(x0) = error_vector(x0)
LieVector hprior(const Pose2& p1) {
return LieVector(sigmas->whiten(priorFactor.evaluateError(p1)));
}
/* ************************************************************************* */
TEST_UNSAFE( Pose2Prior, linearize )
{
// Choose a linearization point at ground truth
pose2SLAM::Values x0;
x0.insertPose(1, priorVal);
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
boost::shared_ptr<JacobianFactor> actual =
boost::dynamic_pointer_cast<JacobianFactor>(priorFactor.linearize(x0, ordering));
// Test with numerical derivative
Matrix numericalH = numericalDerivative11(hprior, priorVal);
CHECK(assert_equal(numericalH,actual->getA(actual->find(ordering[1]))));
}
/* ************************************************************************* */
// Very simple test establishing Ax-b \approx z-h(x)
TEST_UNSAFE( Pose2Factor, error )
{
// Choose a linearization point
Pose2 p1; // robot at origin
Pose2 p2(1, 0, 0); // robot at (1,0)
pose2SLAM::Values x0;
x0.insertPose(1, p1);
x0.insertPose(2, p2);
// Create factor
Pose2 z = p1.between(p2);
Pose2Factor factor(1, 2, z, covariance);
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
boost::shared_ptr<JacobianFactor> linear =
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(x0, ordering));
// Check error at x0, i.e. delta = zero !
VectorValues delta(x0.dims(ordering));
delta[ordering[1]] = zero(3);
delta[ordering[2]] = zero(3);
Vector error_at_zero = Vector_(3,0.0,0.0,0.0);
CHECK(assert_equal(error_at_zero,factor.unwhitenedError(x0)));
CHECK(assert_equal(-error_at_zero, linear->error_vector(delta)));
// Check error after increasing p2
VectorValues plus = delta;
plus[ordering[2]] = Vector_(3, 0.1, 0.0, 0.0);
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)));
}
/* ************************************************************************* */
// common Pose2Factor for tests below
static Pose2 measured(2,2,M_PI_2);
static Pose2Factor factor(1,2,measured, covariance);
/* ************************************************************************* */
TEST_UNSAFE( 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)
pose2SLAM::Values x0;
x0.insert(1,p1);
x0.insert(2,p2);
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
boost::shared_ptr<JacobianFactor> linear =
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(x0, ordering));
// Check RHS
Pose2 hx0 = p1.between(p2);
CHECK(assert_equal(Pose2(2.1, 2.1, M_PI_2),hx0));
Vector expected_b = Vector_(3, -0.1/sx, 0.1/sy, 0.0);
CHECK(assert_equal(expected_b,-factor.whitenedError(x0)));
CHECK(assert_equal(expected_b,linear->getb()));
}
/* ************************************************************************* */
// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
// Hence i.e., b = approximates z-h(x0) = error_vector(x0)
LieVector h(const Pose2& p1,const Pose2& p2) {
return LieVector(covariance->whiten(factor.evaluateError(p1,p2)));
}
/* ************************************************************************* */
TEST_UNSAFE( Pose2Factor, linearize )
{
// Choose a linearization point at ground truth
Pose2 p1(1,2,M_PI_2);
Pose2 p2(-1,4,M_PI);
pose2SLAM::Values x0;
x0.insert(1,p1);
x0.insert(2,p2);
// expected linearization
Matrix expectedH1 = covariance->Whiten(Matrix_(3,3,
0.0,-1.0,-2.0,
1.0, 0.0,-2.0,
0.0, 0.0,-1.0
));
Matrix expectedH2 = covariance->Whiten(Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0
));
Vector expected_b = Vector_(3, 0.0, 0.0, 0.0);
// expected linear factor
Ordering ordering(*x0.orderingArbitrary());
SharedDiagonal probModel1 = noiseModel::Unit::Create(3);
JacobianFactor expected(ordering[1], expectedH1, ordering[2], expectedH2, expected_b, probModel1);
// Actual linearization
boost::shared_ptr<JacobianFactor> actual =
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(x0, ordering));
CHECK(assert_equal(expected,*actual));
// Numerical do not work out because BetweenFactor is approximate ?
Matrix numericalH1 = numericalDerivative21(h, p1, p2);
CHECK(assert_equal(expectedH1,numericalH1));
Matrix numericalH2 = numericalDerivative22(h, p1, p2);
CHECK(assert_equal(expectedH2,numericalH2));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

211
gtsam/slam/tests/testPose3SLAM.cpp
View File

@ -1,211 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testpose3SLAM.cpp
* @author Frank Dellaert
* @author Viorela Ila
**/
#include <gtsam/slam/pose3SLAM.h>
#include <gtsam/slam/PartialPriorFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp>
#include <boost/assign/std/vector.hpp>
using namespace boost;
using namespace boost::assign;
#include <iostream>
using namespace std;
using namespace gtsam;
// common measurement covariance
static noiseModel::Gaussian::shared_ptr covariance(noiseModel::Unit::Create(6));
const double tol=1e-5;
/* ************************************************************************* */
// test optimization with 6 poses arranged in a hexagon and a loop closure
TEST(Pose3Graph, optimizeCircle) {
// Create a hexagon of poses
double radius = 10;
Values hexagon = pose3SLAM::Values::Circle(6,radius);
Pose3 gT0 = hexagon.at<Pose3>(0), gT1 = hexagon.at<Pose3>(1);
// create a Pose graph with one equality constraint and one measurement
pose3SLAM::Graph fg;
fg.addPoseConstraint(0, gT0);
Pose3 _0T1 = gT0.between(gT1); // inv(gT0)*gT1
double theta = M_PI/3.0;
CHECK(assert_equal(Pose3(Rot3::yaw(-theta),Point3(radius*sin(theta),-radius*cos(theta),0)),_0T1));
fg.addRelativePose(0,1, _0T1, covariance);
fg.addRelativePose(1,2, _0T1, covariance);
fg.addRelativePose(2,3, _0T1, covariance);
fg.addRelativePose(3,4, _0T1, covariance);
fg.addRelativePose(4,5, _0T1, covariance);
fg.addRelativePose(5,0, _0T1, covariance);
// Create initial config
Values initial;
initial.insert(0, gT0);
initial.insert(1, hexagon.at<Pose3>(1).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial.insert(2, hexagon.at<Pose3>(2).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial.insert(3, hexagon.at<Pose3>(3).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial.insert(4, hexagon.at<Pose3>(4).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial.insert(5, hexagon.at<Pose3>(5).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
// Choose an ordering and optimize
Ordering ordering;
ordering += 0,1,2,3,4,5;
Values actual = LevenbergMarquardtOptimizer(fg, initial, ordering).optimize();
// Check with ground truth
CHECK(assert_equal(hexagon, actual,1e-4));
// Check loop closure
CHECK(assert_equal(_0T1, actual.at<Pose3>(5).between(actual.at<Pose3>(0)),1e-5));
}
/* ************************************************************************* */
TEST(Pose3Graph, partial_prior_height) {
typedef PartialPriorFactor<Pose3> Partial;
// reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3)
// height prior - single element interface
double exp_height = 5.0;
SharedDiagonal model = noiseModel::Unit::Create(1);
Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height));
Partial height(1, 5, exp_height, model);
Matrix actA;
EXPECT(assert_equal(Vector_(1,-2.0), height.evaluateError(init, actA), tol));
Matrix expA = Matrix_(1, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
EXPECT(assert_equal(expA, actA));
pose3SLAM::Graph graph;
graph.add(height);
Values values;
values.insert(1, init);
// linearization
EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol);
Values actual = LevenbergMarquardtOptimizer(graph, values).optimize();
EXPECT(assert_equal(expected, actual.at<Pose3>(1), tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
}
/* ************************************************************************* */
TEST( Pose3Factor, error )
{
// Create example
Pose3 t1; // origin
Pose3 t2(Rot3::rodriguez(0.1,0.2,0.3),Point3(0,1,0));
Pose3 z(Rot3::rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));;
// Create factor
SharedNoiseModel I6(noiseModel::Unit::Create(6));
BetweenFactor<Pose3> factor(1, 2, z, I6);
// Create config
Values x;
x.insert(1,t1);
x.insert(2,t2);
// Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2))
Vector actual = factor.unwhitenedError(x);
Vector expected = z.localCoordinates(t1.between(t2));
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST(Pose3Graph, partial_prior_xy) {
typedef PartialPriorFactor<Pose3> Partial;
// XY prior - full mask interface
Vector exp_xy = Vector_(2, 3.0, 4.0);
SharedDiagonal model = noiseModel::Unit::Create(2);
Pose3 init(Rot3(), Point3(1.0,-2.0, 3.0)), expected(Rot3(), Point3(3.0, 4.0, 3.0));
vector<size_t> mask; mask += 3, 4;
Partial priorXY(1, mask, exp_xy, model);
Matrix actA;
EXPECT(assert_equal(Vector_(2,-2.0,-6.0), priorXY.evaluateError(init, actA), tol));
Matrix expA = Matrix_(2, 6,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
EXPECT(assert_equal(expA, actA));
pose3SLAM::Graph graph;
graph.add(priorXY);
Values values;
values.insert(1, init);
Values actual = LevenbergMarquardtOptimizer(graph, values).optimize();
EXPECT(assert_equal(expected, actual.at<Pose3>(1), tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
}
// The world coordinate system has z pointing up, y north, x east
// The vehicle has X forward, Y right, Z down
Rot3 R1(Point3( 0, 1, 0), Point3( 1, 0, 0), Point3(0, 0, -1));
Rot3 R2(Point3(-1, 0, 0), Point3( 0, 1, 0), Point3(0, 0, -1));
Rot3 R3(Point3( 0,-1, 0), Point3(-1, 0, 0), Point3(0, 0, -1));
Rot3 R4(Point3( 1, 0, 0), Point3( 0,-1, 0), Point3(0, 0, -1));
/* ************************************************************************* */
TEST( Values, pose3Circle )
{
// expected is 4 poses tangent to circle with radius 1m
Values expected;
expected.insert(0, Pose3(R1, Point3( 1, 0, 0)));
expected.insert(1, Pose3(R2, Point3( 0, 1, 0)));
expected.insert(2, Pose3(R3, Point3(-1, 0, 0)));
expected.insert(3, Pose3(R4, Point3( 0,-1, 0)));
Values actual = pose3SLAM::Values::Circle(4,1.0);
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( Values, expmap )
{
Values expected;
expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0)));
expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0)));
expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0)));
expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0)));
Ordering ordering(*expected.orderingArbitrary());
VectorValues delta(expected.dims(ordering));
delta.vector() = Vector_(24,
0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0);
Values actual = pose3SLAM::Values::Circle(4,1.0).retract(delta, ordering);
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

193
gtsam/slam/tests/testSparseBA.cpp
View File

@ -1,193 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testsparseBA.cpp
* @brief
* @date Jul 5, 2012
* @author Yong-Dian Jian
*/
#include <gtsam/slam/sparseBA.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/shared_ptr.hpp>
using namespace std;
using namespace boost;
using namespace gtsam;
/* ************************************************************************* */
static SharedNoiseModel sigma(noiseModel::Unit::Create(2));
// Convenience for named keys
using symbol_shorthand::X; /* pose3 */
using symbol_shorthand::K; /* calibration */
using symbol_shorthand::C; /* camera = [pose calibration] */
using symbol_shorthand::L; /* point3 */
static Point3 landmark1(-1.0,-1.0, 0.0);
static Point3 landmark2(-1.0, 1.0, 0.0);
static Point3 landmark3( 1.0, 1.0, 0.0);
static Point3 landmark4( 1.0,-1.0, 0.0);
static Pose3 pose1(Matrix_(3,3,
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.),
Point3(0,0,6.25));
static Pose3 pose2(Matrix_(3,3,
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.),
Point3(0,0,5.00));
static Cal3_S2 calib1 (625, 625, 0, 0, 0);
static Cal3_S2 calib2 (625, 625, 0, 0, 0);
typedef PinholeCamera<Cal3_S2> Camera;
static Camera camera1(pose1, calib1);
static Camera camera2(pose2, calib2);
/* ************************************************************************* */
sparseBA::Graph testGraph1() {
Point2 z11(-100, 100);
Point2 z12(-100,-100);
Point2 z13( 100,-100);
Point2 z14( 100, 100);
Point2 z21(-125, 125);
Point2 z22(-125,-125);
Point2 z23( 125,-125);
Point2 z24( 125, 125);
sparseBA::Graph g;
g.addMeasurement<Camera>(z11, sigma, C(1), L(1));
g.addMeasurement<Camera>(z12, sigma, C(1), L(2));
g.addMeasurement<Camera>(z13, sigma, C(1), L(3));
g.addMeasurement<Camera>(z14, sigma, C(1), L(4));
g.addMeasurement<Camera>(z21, sigma, C(2), L(1));
g.addMeasurement<Camera>(z22, sigma, C(2), L(2));
g.addMeasurement<Camera>(z23, sigma, C(2), L(3));
g.addMeasurement<Camera>(z24, sigma, C(2), L(4));
return g;
}
sparseBA::Graph testGraph2() {
Point2 z11(-100, 100);
Point2 z12(-100,-100);
Point2 z13( 100,-100);
Point2 z14( 100, 100);
Point2 z21(-125, 125);
Point2 z22(-125,-125);
Point2 z23( 125,-125);
Point2 z24( 125, 125);
sparseBA::Graph g;
g.addMeasurement<Cal3_S2>(z11, sigma, X(1), L(1), K(1));
g.addMeasurement<Cal3_S2>(z12, sigma, X(1), L(2), K(1));
g.addMeasurement<Cal3_S2>(z13, sigma, X(1), L(3), K(1));
g.addMeasurement<Cal3_S2>(z14, sigma, X(1), L(4), K(1));
g.addMeasurement<Cal3_S2>(z21, sigma, X(2), L(1), K(1));
g.addMeasurement<Cal3_S2>(z22, sigma, X(2), L(2), K(1));
g.addMeasurement<Cal3_S2>(z23, sigma, X(2), L(3), K(1));
g.addMeasurement<Cal3_S2>(z24, sigma, X(2), L(4), K(1));
return g;
}
/* ************************************************************************* */
TEST( optimizeLM1, sparseBA )
{
// build a graph
sparseBA::Graph graph(testGraph1());
// add 3 landmark constraints
graph.addPointConstraint(L(1), landmark1);
graph.addPointConstraint(L(2), landmark2);
graph.addPointConstraint(L(3), landmark3);
// Create an initial values structure corresponding to the ground truth
Values initialEstimate;
initialEstimate.insert(C(1), camera1);
initialEstimate.insert(C(2), camera2);
initialEstimate.insert(L(1), landmark1);
initialEstimate.insert(L(2), landmark2);
initialEstimate.insert(L(3), landmark3);
initialEstimate.insert(L(4), landmark4);
// Create an ordering of the variables
Ordering ordering;
ordering += L(1),L(2),L(3),L(4),C(1),C(2);
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started
// with the ground truth
optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct
CHECK(assert_equal(initialEstimate, optimizer.values()));
}
/* ************************************************************************* */
TEST( optimizeLM2, sparseBA )
{
// build a graph
sparseBA::Graph graph(testGraph2());
// add 3 landmark constraints
graph.addPointConstraint(L(1), landmark1);
graph.addPointConstraint(L(2), landmark2);
graph.addPointConstraint(L(3), landmark3);
// Create an initial values structure corresponding to the ground truth
Values initialEstimate;
initialEstimate.insert(X(1), pose1);
initialEstimate.insert(X(2), pose2);
initialEstimate.insert(L(1), landmark1);
initialEstimate.insert(L(2), landmark2);
initialEstimate.insert(L(3), landmark3);
initialEstimate.insert(L(4), landmark4);
initialEstimate.insert(K(1), calib2);
// Create an ordering of the variables
Ordering ordering;
ordering += L(1),L(2),L(3),L(4),X(1),X(2),K(1);
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started
// with the ground truth
optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct
CHECK(assert_equal(initialEstimate, optimizer.values()));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

344
gtsam/slam/tests/testVisualSLAM.cpp
View File

@ -1,344 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testVisualSLAM.cpp
* @brief Unit test for two cameras and four landmarks, single camera
* @author Chris Beall
* @author Frank Dellaert
* @author Viorela Ila
*/
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Symbol.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/shared_ptr.hpp>
using namespace boost;
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
static Point3 landmark1(-1.0,-1.0, 0.0);
static Point3 landmark2(-1.0, 1.0, 0.0);
static Point3 landmark3( 1.0, 1.0, 0.0);
static Point3 landmark4( 1.0,-1.0, 0.0);
static Pose3 pose1(Matrix_(3,3,
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
),
Point3(0,0,6.25));
static Pose3 pose2(Matrix_(3,3,
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
),
Point3(0,0,5.00));
/* ************************************************************************* */
visualSLAM::Graph testGraph() {
Point2 z11(-100, 100);
Point2 z12(-100,-100);
Point2 z13( 100,-100);
Point2 z14( 100, 100);
Point2 z21(-125, 125);
Point2 z22(-125,-125);
Point2 z23( 125,-125);
Point2 z24( 125, 125);
shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0));
visualSLAM::Graph g;
g.addMeasurement(z11, sigma, X(1), L(1), sK);
g.addMeasurement(z12, sigma, X(1), L(2), sK);
g.addMeasurement(z13, sigma, X(1), L(3), sK);
g.addMeasurement(z14, sigma, X(1), L(4), sK);
g.addMeasurement(z21, sigma, X(2), L(1), sK);
g.addMeasurement(z22, sigma, X(2), L(2), sK);
g.addMeasurement(z23, sigma, X(2), L(3), sK);
g.addMeasurement(z24, sigma, X(2), L(4), sK);
return g;
}
/* ************************************************************************* */
TEST( VisualSLAM, optimizeLM)
{
// build a graph
visualSLAM::Graph graph(testGraph());
// add 3 landmark constraints
graph.addPointConstraint(L(1), landmark1);
graph.addPointConstraint(L(2), landmark2);
graph.addPointConstraint(L(3), landmark3);
// Create an initial values structure corresponding to the ground truth
Values initialEstimate;
initialEstimate.insert(X(1), pose1);
initialEstimate.insert(X(2), pose2);
initialEstimate.insert(L(1), landmark1);
initialEstimate.insert(L(2), landmark2);
initialEstimate.insert(L(3), landmark3);
initialEstimate.insert(L(4), landmark4);
// Create an ordering of the variables
Ordering ordering;
ordering += L(1),L(2),L(3),L(4),X(1),X(2);
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started
// with the ground truth
optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct
CHECK(assert_equal(initialEstimate, optimizer.values()));
}
/* ************************************************************************* */
TEST( VisualSLAM, optimizeLM2)
{
// build a graph
visualSLAM::Graph graph(testGraph());
// add 2 camera constraints
graph.addPoseConstraint(X(1), pose1);
graph.addPoseConstraint(X(2), pose2);
// Create an initial values structure corresponding to the ground truth
Values initialEstimate;
initialEstimate.insert(X(1), pose1);
initialEstimate.insert(X(2), pose2);
initialEstimate.insert(L(1), landmark1);
initialEstimate.insert(L(2), landmark2);
initialEstimate.insert(L(3), landmark3);
initialEstimate.insert(L(4), landmark4);
// Create an ordering of the variables
Ordering ordering;
ordering += L(1),L(2),L(3),L(4),X(1),X(2);
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering);
EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started
// with the ground truth
optimizer.iterate();
EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct
CHECK(assert_equal(initialEstimate, optimizer.values()));
}
/* ************************************************************************* */
TEST( VisualSLAM, LMoptimizer)
{
// build a graph
visualSLAM::Graph graph(testGraph());
// add 2 camera constraints
graph.addPoseConstraint(X(1), pose1);
graph.addPoseConstraint(X(2), pose2);
// Create an initial values structure corresponding to the ground truth
Values initialEstimate;
initialEstimate.insert(X(1), pose1);
initialEstimate.insert(X(2), pose2);
initialEstimate.insert(L(1), landmark1);
initialEstimate.insert(L(2), landmark2);
initialEstimate.insert(L(3), landmark3);
initialEstimate.insert(L(4), landmark4);
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
LevenbergMarquardtOptimizer optimizer = graph.optimizer(initialEstimate);
EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started
// with the ground truth
optimizer.iterate();
EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct
CHECK(assert_equal(initialEstimate, optimizer.values()));
// check errors
Matrix errors = graph.reprojectionErrors(optimizer.values());
CHECK(assert_equal(zeros(2,8), errors));
}
/* ************************************************************************* */
TEST( VisualSLAM, CHECK_ORDERING)
{
// build a graph
visualSLAM::Graph graph = testGraph();
// add 2 camera constraints
graph.addPoseConstraint(X(1), pose1);
graph.addPoseConstraint(X(2), pose2);
// Create an initial values structure corresponding to the ground truth
Values initialEstimate;
initialEstimate.insert(X(1), pose1);
initialEstimate.insert(X(2), pose2);
initialEstimate.insert(L(1), landmark1);
initialEstimate.insert(L(2), landmark2);
initialEstimate.insert(L(3), landmark3);
initialEstimate.insert(L(4), landmark4);
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started
// with the ground truth
optimizer.iterate();
EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct
CHECK(assert_equal(initialEstimate, optimizer.values()));
}
/* ************************************************************************* */
TEST( VisualSLAM, update_with_large_delta) {
// this test ensures that if the update for delta is larger than
// the size of the config, it only updates existing variables
Values init;
init.insert(X(1), Pose3());
init.insert(L(1), Point3(1.0, 2.0, 3.0));
Values expected;
expected.insert(X(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
expected.insert(L(1), Point3(1.1, 2.1, 3.1));
Ordering largeOrdering;
Values largeValues = init;
largeValues.insert(X(2), Pose3());
largeOrdering += X(1),L(1),X(2);
VectorValues delta(largeValues.dims(largeOrdering));
delta[largeOrdering[X(1)]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1);
delta[largeOrdering[L(1)]] = Vector_(3, 0.1, 0.1, 0.1);
delta[largeOrdering[X(2)]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1);
Values actual = init.retract(delta, largeOrdering);
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( VisualSLAM, dataAssociation) {
visualSLAM::ISAM isam;
// add two landmarks
// add two cameras and constraint and odometry
// std::pair<visualSLAM::Values,GaussianBayesNet> actualJoint = isam.jointPrediction(); // 4*4
// std::pair<visualSLAM::Values,GaussianBayesNet> actualMarginals = isam.individualPredictions(); // 2 times 2*2
// std::pair<visualSLAM::Values,GaussianBayesNet> actualChowLiu = isam.chowLiuPredictions(); // 2 times 2*2
}
/* ************************************************************************* */
TEST( VisualSLAM, filteredValues) {
visualSLAM::Values full;
full.insert(X(1), Pose3(Pose2(1.0, 2.0, 0.3)));
full.insert(L(1), Point3(1.0, 2.0, 3.0));
visualSLAM::Values actPoses(full.allPoses());
visualSLAM::Values expPoses; expPoses.insert(X(1), Pose3(Pose2(1.0, 2.0, 0.3)));
EXPECT(assert_equal(expPoses, actPoses));
visualSLAM::Values actPoints(full.allPoints());
visualSLAM::Values expPoints; expPoints.insert(L(1), Point3(1.0, 2.0, 3.0));
EXPECT(assert_equal(expPoints, actPoints));
}
/* ************************************************************************* */
TEST( VisualSLAM, keys_and_view )
{
// create config
visualSLAM::Values c;
c.insert(X(1), pose1);
c.insert(X(2), pose2);
c.insert(L(2), landmark2);
LONGS_EQUAL(2,c.nrPoses());
LONGS_EQUAL(1,c.nrPoints());
{
FastList<Key> expected, actual;
expected += L(2), X(1), X(2);
actual = c.keys();
CHECK(expected == actual);
}
{
FastList<Key> expected, actual;
expected += X(1), X(2);
actual = c.poseKeys();
CHECK(expected == actual);
}
{
FastList<Key> expected, actual;
expected += L(2);
actual = c.pointKeys();
CHECK(expected == actual);
}
}
/* ************************************************************************* */
TEST( VisualSLAM, addMeasurements )
{
// create config
visualSLAM::Graph g;
Vector J = Vector_(3,1.0,2.0,3.0);
Matrix Z = Matrix_(2,3, -1.0,0.0,1.0, -1.0,0.0,1.0);
shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0));
g.addMeasurements(0,J,Z,sigma,sK);
EXPECT_LONGS_EQUAL(3,g.size());
}
/* ************************************************************************* */
TEST( VisualSLAM, insertBackProjections )
{
// create config
visualSLAM::Values c;
SimpleCamera camera(pose1);
Vector J = Vector_(3,1.0,2.0,3.0);
Matrix Z = Matrix_(2,3, -1.0,0.0,1.0, -1.0,0.0,1.0);
c.insertBackprojections(camera,J,Z,1.0);
EXPECT_LONGS_EQUAL(3,c.nrPoints());
}
/* ************************************************************************* */
TEST( VisualSLAM, perturbPoints )
{
visualSLAM::Values c1,c2;
c1.insert(L(2), landmark2);
c1.perturbPoints(0.01,42u);
CHECK(assert_equal(Point3(-0.986984, 0.999534, -0.0147962),c1.point(L(2)),1e-6));
c2.insert(L(2), landmark2);
c2.perturbPoints(0.01,42u);
CHECK(assert_equal(Point3(-0.986984, 0.999534, -0.0147962),c2.point(L(2)),1e-6));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

116
gtsam/slam/visualSLAM.cpp
View File

@ -1,116 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file visualSLAM.cpp
* @date Jan 14, 2010
* @author richard
*/
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/Sampler.h>
#include <boost/make_shared.hpp>
namespace visualSLAM {
using boost::make_shared;
/* ************************************************************************* */
void Values::insertBackprojections(const SimpleCamera& camera,
const Vector& J, const Matrix& Z, double depth) {
if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K");
if (Z.cols() != J.size()) throw std::invalid_argument(
"insertBackProjections: J and Z must have same number of entries");
for(int k=0;k<Z.cols();k++) {
Point2 p(Z(0,k),Z(1,k));
Point3 P = camera.backproject(p, depth);
insertPoint(J(k), P);
}
}
/* ************************************************************************* */
void Values::perturbPoints(double sigma, int32_t seed) {
ConstFiltered<Point3> points = allPoints();
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma);
Sampler sampler(model, seed);
BOOST_FOREACH(const ConstFiltered<Point3>::KeyValuePair& keyValue, points) {
update(keyValue.key, keyValue.value.retract(sampler.sample()));
}
}
/* ************************************************************************* */
Matrix Values::points() const {
size_t j=0;
ConstFiltered<Point3> points = allPoints();
Matrix result(points.size(),3);
BOOST_FOREACH(const ConstFiltered<Point3>::KeyValuePair& keyValue, points)
result.row(j++) = keyValue.value.vector();
return result;
}
/* ************************************************************************* */
void Graph::addPointConstraint(Key pointKey, const Point3& p) {
push_back(make_shared<NonlinearEquality<Point3> >(pointKey, p));
}
/* ************************************************************************* */
void Graph::addPointPrior(Key pointKey, const Point3& p, const SharedNoiseModel& model) {
push_back(make_shared<PriorFactor<Point3> >(pointKey, p, model));
}
/* ************************************************************************* */
void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrK K) {
push_back(
make_shared<GenericProjectionFactor<Pose3, Point3> >
(measured, model, poseKey, pointKey, K));
}
/* ************************************************************************* */
void Graph::addMeasurements(Key i, const Vector& J, const Matrix& Z,
const SharedNoiseModel& model, const shared_ptrK K) {
if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K");
if (Z.cols() != J.size()) throw std::invalid_argument(
"addMeasurements: J and Z must have same number of entries");
for (int k = 0; k < Z.cols(); k++)
addMeasurement(Point2(Z(0, k), Z(1, k)), model, i, J(k), K);
}
/* ************************************************************************* */
void Graph::addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrKStereo K) {
push_back(make_shared<GenericStereoFactor<Pose3, Point3> >(measured, model, poseKey, pointKey, K));
}
/* ************************************************************************* */
void Graph::addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model) {
push_back(make_shared<gtsam::RangeFactor<Pose3, Point3> >(poseKey, pointKey, range, model));
}
/* ************************************************************************* */
Matrix Graph::reprojectionErrors(const Values& values) const {
// first count
size_t K = 0, k=0;
BOOST_FOREACH(const sharedFactor& f, *this)
if (boost::dynamic_pointer_cast<const ProjectionFactor>(f)) ++K;
// now fill
Matrix errors(2,K);
BOOST_FOREACH(const sharedFactor& f, *this) {
boost::shared_ptr<const ProjectionFactor> p =
boost::dynamic_pointer_cast<const ProjectionFactor>(f);
if (p) errors.col(k++) = p->unwhitenedError(values);
}
return errors;
}
/* ************************************************************************* */
}

186
gtsam/slam/visualSLAM.h
View File

@ -1,186 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file visualSLAM.h
* @brief Basic typedefs for general VisualSLAM problems. Useful for monocular and stereo systems.
* @date Jan 14, 2010
* @author Richard Roberts
* @author Chris Beall
*/
#pragma once
#include <gtsam/slam/pose3SLAM.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/nonlinear/NonlinearISAM.h>
#include <gtsam/geometry/SimpleCamera.h>
namespace visualSLAM {
using namespace gtsam;
/// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
struct Values: public pose3SLAM::Values {
typedef boost::shared_ptr<Values> shared_ptr;
typedef gtsam::Values::ConstFiltered<Pose3> PoseFiltered;
typedef gtsam::Values::ConstFiltered<Point3> PointFiltered;
/// Default constructor
Values() {}
/// Copy constructor
Values(const gtsam::Values& values) :
pose3SLAM::Values(values) {
}
/// Constructor from filtered values view of poses
Values(const PoseFiltered& view) : pose3SLAM::Values(view) {}
/// Constructor from filtered values view of points
Values(const PointFiltered& view) : pose3SLAM::Values(view) {}
PoseFiltered allPoses() const { return this->filter<Pose3>(); } ///< pose view
size_t nrPoses() const { return allPoses().size(); } ///< get number of poses
KeyList poseKeys() const { return allPoses().keys(); } ///< get keys to poses only
PointFiltered allPoints() const { return this->filter<Point3>(); } ///< point view
size_t nrPoints() const { return allPoints().size(); } ///< get number of points
KeyList pointKeys() const { return allPoints().keys(); } ///< get keys to points only
/// insert a point
void insertPoint(Key j, const Point3& point) { insert(j, point); }
/// update a point
void updatePoint(Key j, const Point3& point) { update(j, point); }
/// get a point
Point3 point(Key j) const { return at<Point3>(j); }
/// insert a number of initial point values by backprojecting
void insertBackprojections(const SimpleCamera& c, const Vector& J,
const Matrix& Z, double depth);
/// perturb all points using normally distributed noise
void perturbPoints(double sigma, int32_t seed = 42u);
/// get all point coordinates in a matrix
Matrix points() const;
};
/**
* Non-linear factor graph for vanilla visual SLAM
*/
struct Graph: public pose3SLAM::Graph {
/// shared pointer to this type of graph
typedef boost::shared_ptr<Graph> shared_graph;
/// Default constructor
Graph(){}
/// Copy constructor given any other NonlinearFactorGraph
Graph(const NonlinearFactorGraph& graph):
pose3SLAM::Graph(graph) {}
/// check if two graphs are equal
bool equals(const Graph& p, double tol = 1e-9) const {
return NonlinearFactorGraph::equals(p, tol);
}
/**
* Add a constraint on a point (for now, *must* be satisfied in any Values)
* @param key variable key of the point
* @param p point around which soft prior is defined
*/
void addPointConstraint(Key pointKey, const Point3& p = Point3());
/**
* Add a prior on a point
* @param key variable key of the point
* @param p to which point to constrain it to
* @param model uncertainty model of this prior
*/
void addPointPrior(Key pointKey, const Point3& p = Point3(),
const SharedNoiseModel& model = noiseModel::Unit::Create(3));
/**
* Add a range prior to a point
* @param poseKey variable key of the camera pose
* @param pointKey variable key of the point
* @param range approximate range to point
* @param model uncertainty model of this prior
*/
void addRangeFactor(Key poseKey, Key pointKey, double range,
const SharedNoiseModel& model = noiseModel::Unit::Create(1));
/**
* Add a projection factor measurement (monocular)
* @param measured the measurement
* @param model the noise model for the measurement
* @param poseKey variable key for the camera pose
* @param pointKey variable key for the point
* @param K shared pointer to calibration object
*/
void addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrK K);
/**
* Add a number of measurements at the same time
* @param i variable key for the camera pose
* @param J variable keys for the point, KeyVector of size K
* @param Z the 2*K measurements as a matrix
* @param model the noise model for the measurement
* @param K shared pointer to calibration object
*/
void addMeasurements(Key i, const Vector& J, const Matrix& Z,
const SharedNoiseModel& model, const shared_ptrK K);
/**
* Add a stereo factor measurement
* @param measured the measurement
* @param model the noise model for the measurement
* @param poseKey variable key for the camera pose
* @param pointKey variable key for the point
* @param K shared pointer to stereo calibration object
*/
void addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrKStereo K);
/// Return a 2*K Matrix of reprojection errors
Matrix reprojectionErrors(const Values& values) const;
}; // Graph
/**
* Non-linear ISAM for vanilla incremental visual SLAM inference
*/
typedef gtsam::NonlinearISAM ISAM;
} // visualSLAM
/**
* Backwards compatibility and wrap use only, avoid using
*/
namespace visualSLAM {
typedef gtsam::NonlinearEquality<Pose3> PoseConstraint; ///< \deprecated typedef for backwards compatibility
typedef gtsam::NonlinearEquality<Point3> PointConstraint; ///< \deprecated typedef for backwards compatibility
typedef gtsam::PriorFactor<Pose3> PosePrior; ///< \deprecated typedef for backwards compatibility
typedef gtsam::PriorFactor<Point3> PointPrior; ///< \deprecated typedef for backwards compatibility
typedef gtsam::RangeFactor<Pose3, Point3> RangeFactor; ///< \deprecated typedef for backwards compatibility
typedef gtsam::GenericProjectionFactor<Pose3, Point3> ProjectionFactor; ///< \deprecated typedef for backwards compatibility
typedef gtsam::GenericStereoFactor<Pose3, Point3> StereoFactor; ///< \deprecated typedef for backwards compatibility
}