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

most tests passed, except testPose2SLAMwSPCG, testGaussianISAM2, testNonlinearEquality, testNonlinearFactorGraph, testProjectionFactor, testVSLAM

release/4.3a0
Duy-Nguyen Ta 2012-01-30 04:34:46 +00:00
parent 2db224df3c
commit 98f2d47f58
29 changed files with 434 additions and 442 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
gtsam/inference/graph-inl.h
View File

@ -139,19 +139,19 @@ predecessorMap2Graph(const PredecessorMap<KEY>& p_map) {
}
/* ************************************************************************* */
template <class V,class POSE, class VALUES>
template <class V, class POSE, class KEY>
class compose_key_visitor : public boost::default_bfs_visitor {
private:
boost::shared_ptr<VALUES> config_;
boost::shared_ptr<DynamicValues> config_;
public:
compose_key_visitor(boost::shared_ptr<VALUES> config_in) {config_ = config_in;}
compose_key_visitor(boost::shared_ptr<DynamicValues> config_in) {config_ = config_in;}
template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
typename VALUES::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
typename VALUES::Key key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
KEY key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
POSE relativePose = boost::get(boost::edge_weight, g, edge);
config_->insert(key_to, (*config_)[key_from].compose(relativePose));
}
@ -159,23 +159,23 @@ public:
};
/* ************************************************************************* */
template<class G, class Factor, class POSE, class VALUES>
boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<typename VALUES::Key>& tree,
template<class G, class Factor, class POSE, class KEY>
boost::shared_ptr<DynamicValues> composePoses(const G& graph, const PredecessorMap<KEY>& tree,
const POSE& rootPose) {
//TODO: change edge_weight_t to edge_pose_t
typedef typename boost::adjacency_list<
boost::vecS, boost::vecS, boost::directedS,
boost::property<boost::vertex_name_t, typename VALUES::Key>,
boost::property<boost::vertex_name_t, KEY>,
boost::property<boost::edge_weight_t, POSE> > PoseGraph;
typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex;
typedef typename boost::graph_traits<PoseGraph>::edge_descriptor PoseEdge;
PoseGraph g;
PoseVertex root;
map<typename VALUES::Key, PoseVertex> key2vertex;
map<KEY, PoseVertex> key2vertex;
boost::tie(g, root, key2vertex) =
predecessorMap2Graph<PoseGraph, PoseVertex, typename VALUES::Key>(tree);
predecessorMap2Graph<PoseGraph, PoseVertex, KEY>(tree);
// attach the relative poses to the edges
PoseEdge edge12, edge21;
@ -189,8 +189,8 @@ boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<type
boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
if (!factor) continue;
typename VALUES::Key key1 = factor->key1();
typename VALUES::Key key2 = factor->key2();
KEY key1 = factor->key1();
KEY key2 = factor->key2();
PoseVertex v1 = key2vertex.find(key1)->second;
PoseVertex v2 = key2vertex.find(key2)->second;
@ -207,10 +207,10 @@ boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<type
}
// compose poses
boost::shared_ptr<VALUES> config(new VALUES);
typename VALUES::Key rootKey = boost::get(boost::vertex_name, g, root);
boost::shared_ptr<DynamicValues> config(new DynamicValues);
KEY rootKey = boost::get(boost::vertex_name, g, root);
config->insert(rootKey, rootPose);
compose_key_visitor<PoseVertex, POSE, VALUES> vis(config);
compose_key_visitor<PoseVertex, POSE, KEY> vis(config);
boost::breadth_first_search(g, root, boost::visitor(vis));
return config;

7
gtsam/inference/graph.h
View File

@ -25,6 +25,7 @@
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/shared_ptr.hpp>
#include <gtsam/nonlinear/DynamicValues.h>
namespace gtsam {
@ -87,9 +88,9 @@ namespace gtsam {
/**
* Compose the poses by following the chain specified by the spanning tree
*/
template<class G, class Factor, class POSE, class VALUES>
boost::shared_ptr<VALUES>
composePoses(const G& graph, const PredecessorMap<typename VALUES::Key>& tree, const POSE& rootPose);
template<class G, class Factor, class POSE, class KEY>
boost::shared_ptr<DynamicValues>
composePoses(const G& graph, const PredecessorMap<KEY>& tree, const POSE& rootPose);
/**

4
gtsam/nonlinear/Makefile.am
View File

@ -16,9 +16,9 @@ check_PROGRAMS =
#----------------------------------------------------------------------------------------------------
# Lie Groups
headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h DynamicValues-inl.h
headers += DynamicValues-inl.h
sources += DynamicValues.cpp
check_PROGRAMS += tests/testValues tests/testDynamicValues tests/testKey tests/testOrdering tests/testNonlinearFactor
check_PROGRAMS += tests/testDynamicValues tests/testKey tests/testOrdering tests/testNonlinearFactor
# Nonlinear nonlinear
headers += Key.h

10
gtsam/slam/BetweenFactor.h
View File

@ -114,15 +114,15 @@ namespace gtsam {
* This constraint requires the underlying type to a Lie type
*
*/
template<class VALUES, class KEY>
class BetweenConstraint : public BetweenFactor<VALUES, KEY> {
template<class KEY>
class BetweenConstraint : public BetweenFactor<KEY> {
public:
typedef boost::shared_ptr<BetweenConstraint<VALUES, KEY> > shared_ptr;
typedef boost::shared_ptr<BetweenConstraint<KEY> > shared_ptr;
/** Syntactic sugar for constrained version */
BetweenConstraint(const typename KEY::Value& measured,
const KEY& key1, const KEY& key2, double mu = 1000.0)
: BetweenFactor<VALUES, KEY>(key1, key2, measured,
: BetweenFactor<KEY>(key1, key2, measured,
noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {}
private:
@ -132,7 +132,7 @@ namespace gtsam {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("BetweenFactor",
boost::serialization::base_object<BetweenFactor<VALUES, KEY> >(*this));
boost::serialization::base_object<BetweenFactor<KEY> >(*this));
}
}; // \class BetweenConstraint

12
gtsam/slam/GeneralSFMFactor.h
View File

@ -28,21 +28,21 @@ namespace gtsam {
/**
* Non-linear factor for a constraint derived from a 2D measurement. The calibration is unknown here compared to GenericProjectionFactor
*/
template <class VALUES, class CamK, class LmK>
template <class CamK, class LmK>
class GeneralSFMFactor:
public NonlinearFactor2<VALUES, CamK, LmK> {
public NonlinearFactor2<CamK, LmK> {
protected:
Point2 z_; ///< the 2D measurement
public:
typedef typename CamK::Value Cam; ///< typedef for camera type
typedef GeneralSFMFactor<VALUES, CamK, LmK> Self ; ///< typedef for this object
typedef NonlinearFactor2<VALUES, CamK, LmK> Base; ///< typedef for the base class
typedef GeneralSFMFactor<CamK, LmK> Self ; ///< typedef for this object
typedef NonlinearFactor2<CamK, LmK> Base; ///< typedef for the base class
typedef Point2 Measurement; ///< typedef for the measurement
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GeneralSFMFactor<VALUES, LmK, CamK> > shared_ptr;
typedef boost::shared_ptr<GeneralSFMFactor<LmK, CamK> > shared_ptr;
/**
* Constructor
@ -71,7 +71,7 @@ namespace gtsam {
/**
* equals
*/
bool equals(const GeneralSFMFactor<VALUES, CamK, LmK> &p, double tol = 1e-9) const {
bool equals(const GeneralSFMFactor<CamK, LmK> &p, double tol = 1e-9) const {
return Base::equals(p, tol) && this->z_.equals(p.z_, tol) ;
}

2
gtsam/slam/Makefile.am
View File

@ -45,7 +45,7 @@ check_PROGRAMS += tests/testPose3SLAM
# Visual SLAM
headers += GeneralSFMFactor.h ProjectionFactor.h
sources += visualSLAM.cpp
check_PROGRAMS += tests/testProjectionFactor tests/testVSLAM
#check_PROGRAMS += tests/testProjectionFactor tests/testVSLAM
check_PROGRAMS += tests/testGeneralSFMFactor tests/testGeneralSFMFactor_Cal3Bundler
# StereoFactor

2
gtsam/slam/PartialPriorFactor.h
View File

@ -95,7 +95,7 @@ namespace gtsam {
}
/** equals */
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) &&
gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) &&

38
gtsam/slam/simulated2DConstraints.h
View File

@ -35,13 +35,13 @@ namespace gtsam {
namespace equality_constraints {
/** Typedefs for regular use */
typedef NonlinearEquality1<Values, PoseKey> UnaryEqualityConstraint;
typedef NonlinearEquality1<Values, PointKey> UnaryEqualityPointConstraint;
typedef BetweenConstraint<Values, PoseKey> OdoEqualityConstraint;
typedef NonlinearEquality1<PoseKey> UnaryEqualityConstraint;
typedef NonlinearEquality1<PointKey> UnaryEqualityPointConstraint;
typedef BetweenConstraint<PoseKey> OdoEqualityConstraint;
/** Equality between variables */
typedef NonlinearEquality2<Values, PoseKey> PoseEqualityConstraint;
typedef NonlinearEquality2<Values, PointKey> PointEqualityConstraint;
typedef NonlinearEquality2<PoseKey> PoseEqualityConstraint;
typedef NonlinearEquality2<PointKey> PointEqualityConstraint;
} // \namespace equality_constraints
@ -53,10 +53,10 @@ namespace gtsam {
* @tparam KEY is the key type for the variable constrained
* @tparam IDX is an index in tangent space to constrain, must be less than KEY::VALUE::Dim()
*/
template<class VALUES, class KEY, unsigned int IDX>
struct ScalarCoordConstraint1: public BoundingConstraint1<VALUES, KEY> {
typedef BoundingConstraint1<VALUES, KEY> Base; ///< Base class convenience typedef
typedef boost::shared_ptr<ScalarCoordConstraint1<VALUES, KEY, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef
template<class KEY, unsigned int IDX>
struct ScalarCoordConstraint1: public BoundingConstraint1<KEY> {
typedef BoundingConstraint1<KEY> Base; ///< Base class convenience typedef
typedef boost::shared_ptr<ScalarCoordConstraint1<KEY, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef
typedef typename KEY::Value Point; ///< Constrained variable type
virtual ~ScalarCoordConstraint1() {}
@ -96,8 +96,8 @@ namespace gtsam {
};
/** typedefs for use with simulated2D systems */
typedef ScalarCoordConstraint1<Values, PoseKey, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X
typedef ScalarCoordConstraint1<Values, PoseKey, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
typedef ScalarCoordConstraint1<PoseKey, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X
typedef ScalarCoordConstraint1<PoseKey, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
/**
* Trait for distance constraints to provide distance
@ -116,9 +116,9 @@ namespace gtsam {
* @tparam VALUES is the variable set for the graph
* @tparam KEY is the type of the keys for the variables constrained
*/
template<class VALUES, class KEY>
struct MaxDistanceConstraint : public BoundingConstraint2<VALUES, KEY, KEY> {
typedef BoundingConstraint2<VALUES, KEY, KEY> Base; ///< Base class for factor
template<class KEY>
struct MaxDistanceConstraint : public BoundingConstraint2<KEY, KEY> {
typedef BoundingConstraint2<KEY, KEY> Base; ///< Base class for factor
typedef typename KEY::Value Point; ///< Type of variable constrained
virtual ~MaxDistanceConstraint() {}
@ -150,7 +150,7 @@ namespace gtsam {
}
};
typedef MaxDistanceConstraint<Values, PoseKey> PoseMaxDistConstraint; ///< Simulated2D domain example factor
typedef MaxDistanceConstraint<PoseKey> PoseMaxDistConstraint; ///< Simulated2D domain example factor
/**
* Binary inequality constraint forcing a minimum range
@ -159,9 +159,9 @@ namespace gtsam {
* @tparam XKEY is the type of the pose key constrained
* @tparam PKEY is the type of the point key constrained
*/
template<class VALUES, class XKEY, class PKEY>
struct MinDistanceConstraint : public BoundingConstraint2<VALUES, XKEY, PKEY> {
typedef BoundingConstraint2<VALUES, XKEY, PKEY> Base; ///< Base class for factor
template<class XKEY, class PKEY>
struct MinDistanceConstraint : public BoundingConstraint2<XKEY, PKEY> {
typedef BoundingConstraint2<XKEY, PKEY> Base; ///< Base class for factor
typedef typename XKEY::Value Pose; ///< Type of pose variable constrained
typedef typename PKEY::Value Point; ///< Type of point variable constrained
@ -195,7 +195,7 @@ namespace gtsam {
}
};
typedef MinDistanceConstraint<Values, PoseKey, PointKey> LandmarkAvoid; ///< Simulated2D domain example factor
typedef MinDistanceConstraint<PoseKey, PointKey> LandmarkAvoid; ///< Simulated2D domain example factor
} // \namespace inequality_constraints

54
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -20,7 +20,6 @@ using namespace boost;
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/inference/graph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/geometry/GeneralCameraT.h>
@ -33,14 +32,11 @@ typedef Cal3_S2Camera GeneralCamera;
//typedef Cal3BundlerCamera GeneralCamera;
typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
typedef TypedSymbol<Point3, 'l'> PointKey;
typedef Values<CameraKey> CameraConfig;
typedef Values<PointKey> PointConfig;
typedef TupleValues2<CameraConfig, PointConfig> VisualValues;
typedef GeneralSFMFactor<VisualValues, CameraKey, PointKey> Projection;
typedef NonlinearEquality<VisualValues, CameraKey> CameraConstraint;
typedef NonlinearEquality<VisualValues, PointKey> Point3Constraint;
typedef GeneralSFMFactor<CameraKey, PointKey> Projection;
typedef NonlinearEquality<CameraKey> CameraConstraint;
typedef NonlinearEquality<PointKey> Point3Constraint;
class Graph: public NonlinearFactorGraph<VisualValues> {
class Graph: public NonlinearFactorGraph {
public:
void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, i, j));
@ -73,7 +69,7 @@ double getGaussian()
return sqrt(-2.0f * (double)log(S) / S) * V1;
}
typedef NonlinearOptimizer<Graph,VisualValues> Optimizer;
typedef NonlinearOptimizer<Graph> Optimizer;
const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
@ -101,12 +97,12 @@ TEST( GeneralSFMFactor, error ) {
boost::shared_ptr<Projection>
factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
// For the following configuration, the factor predicts 320,240
VisualValues values;
DynamicValues values;
Rot3 R;
Point3 t1(0,0,-6);
Pose3 x1(R,t1);
values.insert(1, GeneralCamera(x1));
Point3 l1; values.insert(1, l1);
values.insert(CameraKey(1), GeneralCamera(x1));
Point3 l1; values.insert(PointKey(1), l1);
EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
}
@ -174,15 +170,15 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// add initial
const double noise = baseline*0.1;
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<DynamicValues> values(new DynamicValues);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian());
values->insert(i, pt) ;
values->insert(PointKey(i), pt) ;
}
graph->addCameraConstraint(0, X[0]);
@ -213,9 +209,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
// add initial
const double noise = baseline*0.1;
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<DynamicValues> values(new DynamicValues);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
// add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) {
@ -223,10 +219,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian());
values->insert(i, pt) ;
values->insert(PointKey(i), pt) ;
}
else {
values->insert(i, L[i]) ;
values->insert(PointKey(i), L[i]) ;
}
}
@ -259,16 +255,16 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<DynamicValues> values(new DynamicValues);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian());
//Point3 pt(L[i].x(), L[i].y(), L[i].z());
values->insert(i, pt) ;
values->insert(PointKey(i), pt) ;
}
for ( size_t i = 0 ; i < X.size() ; ++i )
@ -302,7 +298,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<DynamicValues> values(new DynamicValues);
for ( size_t i = 0 ; i < X.size() ; ++i ) {
const double
rot_noise = 1e-5,
@ -310,7 +306,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
focal_noise = 1,
skew_noise = 1e-5;
if ( i == 0 ) {
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
}
else {
@ -321,12 +317,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
skew_noise, // s
trans_noise, trans_noise // ux, uy
) ;
values->insert((int)i, X[i].retract(delta)) ;
values->insert(CameraKey((int)i), X[i].retract(delta)) ;
}
}
for ( size_t i = 0 ; i < L.size() ; ++i ) {
values->insert(i, L[i]) ;
values->insert(PointKey(i), L[i]) ;
}
// fix X0 and all landmarks, allow only the X[1] to move
@ -363,16 +359,16 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
// add initial
const double noise = baseline*0.1;
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<DynamicValues> values(new DynamicValues);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
// add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian());
values->insert(i, pt) ;
values->insert(PointKey(i), pt) ;
}
graph->addCameraConstraint(0, X[0]);

54
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -20,7 +20,6 @@ using namespace boost;
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/inference/graph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/geometry/GeneralCameraT.h>
@ -32,15 +31,12 @@ using namespace gtsam;
typedef Cal3BundlerCamera GeneralCamera;
typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
typedef TypedSymbol<Point3, 'l'> PointKey;
typedef Values<CameraKey> CameraConfig;
typedef Values<PointKey> PointConfig;
typedef TupleValues2<CameraConfig, PointConfig> VisualValues;
typedef GeneralSFMFactor<VisualValues, CameraKey, PointKey> Projection;
typedef NonlinearEquality<VisualValues, CameraKey> CameraConstraint;
typedef NonlinearEquality<VisualValues, PointKey> Point3Constraint;
typedef GeneralSFMFactor<CameraKey, PointKey> Projection;
typedef NonlinearEquality<CameraKey> CameraConstraint;
typedef NonlinearEquality<PointKey> Point3Constraint;
/* ************************************************************************* */
class Graph: public NonlinearFactorGraph<VisualValues> {
class Graph: public NonlinearFactorGraph {
public:
void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, i, j));
@ -73,7 +69,7 @@ double getGaussian()
return sqrt(-2.0f * (double)log(S) / S) * V1;
}
typedef NonlinearOptimizer<Graph,VisualValues> Optimizer;
typedef NonlinearOptimizer<Graph> Optimizer;
const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
@ -101,12 +97,12 @@ TEST( GeneralSFMFactor, error ) {
boost::shared_ptr<Projection>
factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
// For the following configuration, the factor predicts 320,240
VisualValues values;
DynamicValues values;
Rot3 R;
Point3 t1(0,0,-6);
Pose3 x1(R,t1);
values.insert(1, GeneralCamera(x1));
Point3 l1; values.insert(1, l1);
values.insert(CameraKey(1), GeneralCamera(x1));
Point3 l1; values.insert(PointKey(1), l1);
EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
}
@ -174,15 +170,15 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// add initial
const double noise = baseline*0.1;
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<DynamicValues> values(new DynamicValues);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian());
values->insert(i, pt) ;
values->insert(PointKey(i), pt) ;
}
graph->addCameraConstraint(0, X[0]);
@ -213,9 +209,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
// add initial
const double noise = baseline*0.1;
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<DynamicValues> values(new DynamicValues);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
// add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) {
@ -223,10 +219,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian());
values->insert(i, pt) ;
values->insert(PointKey(i), pt) ;
}
else {
values->insert(i, L[i]) ;
values->insert(PointKey(i), L[i]) ;
}
}
@ -259,16 +255,16 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<DynamicValues> values(new DynamicValues);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian());
//Point3 pt(L[i].x(), L[i].y(), L[i].z());
values->insert(i, pt) ;
values->insert(PointKey(i), pt) ;
}
for ( size_t i = 0 ; i < X.size() ; ++i )
@ -302,13 +298,13 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<DynamicValues> values(new DynamicValues);
for ( size_t i = 0 ; i < X.size() ; ++i ) {
const double
rot_noise = 1e-5, trans_noise = 1e-3,
focal_noise = 1, distort_noise = 1e-3;
if ( i == 0 ) {
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
}
else {
@ -317,12 +313,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
trans_noise, trans_noise, trans_noise, // translation
focal_noise, distort_noise, distort_noise // f, k1, k2
) ;
values->insert((int)i, X[i].retract(delta)) ;
values->insert(CameraKey((int)i), X[i].retract(delta)) ;
}
}
for ( size_t i = 0 ; i < L.size() ; ++i ) {
values->insert(i, L[i]) ;
values->insert(PointKey(i), L[i]) ;
}
// fix X0 and all landmarks, allow only the X[1] to move
@ -359,16 +355,16 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
// add initial
const double noise = baseline*0.1;
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<DynamicValues> values(new DynamicValues);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
// add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian());
values->insert(i, pt) ;
values->insert(PointKey(i), pt) ;
}
graph->addCameraConstraint(0, X[0]);

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

@ -23,6 +23,7 @@
using namespace std;
using namespace gtsam;
using namespace planarSLAM;
// some shared test values
static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
@ -50,9 +51,9 @@ TEST( planarSLAM, BearingFactor )
planarSLAM::Bearing factor(2, 3, z, sigma);
// create config
planarSLAM::Values c;
c.insert(2, x2);
c.insert(3, l3);
DynamicValues c;
c.insert(PoseKey(2), x2);
c.insert(PointKey(3), l3);
// Check error
Vector actual = factor.unwhitenedError(c);
@ -78,9 +79,9 @@ TEST( planarSLAM, RangeFactor )
planarSLAM::Range factor(2, 3, z, sigma);
// create config
planarSLAM::Values c;
c.insert(2, x2);
c.insert(3, l3);
DynamicValues c;
c.insert(PoseKey(2), x2);
c.insert(PointKey(3), l3);
// Check error
Vector actual = factor.unwhitenedError(c);
@ -105,9 +106,9 @@ TEST( planarSLAM, BearingRangeFactor )
planarSLAM::BearingRange factor(2, 3, r, b, sigma2);
// create config
planarSLAM::Values c;
c.insert(2, x2);
c.insert(3, l3);
DynamicValues c;
c.insert(PoseKey(2), x2);
c.insert(PointKey(3), l3);
// Check error
Vector actual = factor.unwhitenedError(c);
@ -138,10 +139,10 @@ TEST( planarSLAM, PoseConstraint_equals )
TEST( planarSLAM, constructor )
{
// create config
planarSLAM::Values c;
c.insert(2, x2);
c.insert(3, x3);
c.insert(3, l3);
DynamicValues c;
c.insert(PoseKey(2), x2);
c.insert(PoseKey(3), x3);
c.insert(PointKey(3), l3);
// create graph
planarSLAM::Graph G;
@ -165,8 +166,8 @@ TEST( planarSLAM, constructor )
Vector expected2 = Vector_(1, -0.1);
Vector expected3 = Vector_(1, 0.22);
// Get NoiseModelFactors
FactorGraph<NoiseModelFactor<planarSLAM::Values> > GNM =
*G.dynamicCastFactors<FactorGraph<NoiseModelFactor<planarSLAM::Values> > >();
FactorGraph<NoiseModelFactor > GNM =
*G.dynamicCastFactors<FactorGraph<NoiseModelFactor > >();
EXPECT(assert_equal(expected0, GNM[0]->unwhitenedError(c)));
EXPECT(assert_equal(expected1, GNM[1]->unwhitenedError(c)));
EXPECT(assert_equal(expected2, GNM[2]->unwhitenedError(c)));

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

@ -32,6 +32,7 @@ using namespace boost::assign;
#include <iostream>
using namespace std;
using namespace pose2SLAM;
// common measurement covariance
static double sx=0.5, sy=0.5,st=0.1;
@ -115,9 +116,9 @@ TEST( Pose2SLAM, linearization )
// 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;
config.insert(1,p1);
config.insert(2,p2);
DynamicValues config;
config.insert(Key(1),p1);
config.insert(Key(2),p2);
// Linearize
Ordering ordering(*config.orderingArbitrary());
boost::shared_ptr<FactorGraph<GaussianFactor> > lfg_linearized = graph.linearize(config, ordering);
@ -151,23 +152,23 @@ TEST(Pose2Graph, optimize) {
fg->addConstraint(0, 1, Pose2(1,2,M_PI_2), covariance);
// Create initial config
boost::shared_ptr<Pose2Values> initial(new Pose2Values());
initial->insert(0, Pose2(0,0,0));
initial->insert(1, Pose2(0,0,0));
boost::shared_ptr<DynamicValues> initial(new DynamicValues());
initial->insert(Key(0), Pose2(0,0,0));
initial->insert(Key(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<Pose2Graph> 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;
expected.insert(0, Pose2(0,0,0));
expected.insert(1, Pose2(1,2,M_PI_2));
DynamicValues expected;
expected.insert(Key(0), Pose2(0,0,0));
expected.insert(Key(1), Pose2(1,2,M_PI_2));
CHECK(assert_equal(expected, *optimizer.values()));
}
@ -176,8 +177,8 @@ TEST(Pose2Graph, optimize) {
TEST(Pose2Graph, optimizeThreePoses) {
// Create a hexagon of poses
Pose2Values hexagon = pose2SLAM::circle(3,1.0);
Pose2 p0 = hexagon[0], p1 = hexagon[1];
DynamicValues hexagon = pose2SLAM::circle(3,1.0);
Pose2 p0 = hexagon[Key(0)], p1 = hexagon[Key(1)];
// create a Pose graph with one equality constraint and one measurement
shared_ptr<Pose2Graph> fg(new Pose2Graph);
@ -188,10 +189,10 @@ TEST(Pose2Graph, optimizeThreePoses) {
fg->addConstraint(2, 0, delta, covariance);
// Create initial config
boost::shared_ptr<Pose2Values> initial(new Pose2Values());
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)));
boost::shared_ptr<DynamicValues> initial(new DynamicValues());
initial->insert(Key(0), p0);
initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(3, 0.1,-0.1, 0.1)));
// Choose an ordering
shared_ptr<Ordering> ordering(new Ordering);
@ -202,7 +203,7 @@ TEST(Pose2Graph, optimizeThreePoses) {
pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params);
pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
Pose2Values actual = *optimizer.values();
DynamicValues actual = *optimizer.values();
// Check with ground truth
CHECK(assert_equal(hexagon, actual));
@ -213,8 +214,8 @@ TEST(Pose2Graph, optimizeThreePoses) {
TEST_UNSAFE(Pose2Graph, optimizeCircle) {
// Create a hexagon of poses
Pose2Values hexagon = pose2SLAM::circle(6,1.0);
Pose2 p0 = hexagon[0], p1 = hexagon[1];
DynamicValues hexagon = pose2SLAM::circle(6,1.0);
Pose2 p0 = hexagon[Key(0)], p1 = hexagon[Key(1)];
// create a Pose graph with one equality constraint and one measurement
shared_ptr<Pose2Graph> fg(new Pose2Graph);
@ -228,13 +229,13 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) {
fg->addConstraint(5, 0, delta, covariance);
// Create initial config
boost::shared_ptr<Pose2Values> initial(new Pose2Values());
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)));
initial->insert(3, hexagon[3].retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(4, hexagon[4].retract(Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(5, hexagon[5].retract(Vector_(3,-0.1, 0.1,-0.1)));
boost::shared_ptr<DynamicValues> initial(new DynamicValues());
initial->insert(Key(0), p0);
initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(Key(3), hexagon[Key(3)].retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(Key(4), hexagon[Key(4)].retract(Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(Key(5), hexagon[Key(5)].retract(Vector_(3,-0.1, 0.1,-0.1)));
// Choose an ordering
shared_ptr<Ordering> ordering(new Ordering);
@ -245,13 +246,13 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) {
pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params);
pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
Pose2Values actual = *optimizer.values();
DynamicValues actual = *optimizer.values();
// Check with ground truth
CHECK(assert_equal(hexagon, actual));
// Check loop closure
CHECK(assert_equal(delta,actual[5].between(actual[0])));
CHECK(assert_equal(delta,actual[Key(5)].between(actual[Key(0)])));
// Pose2SLAMOptimizer myOptimizer("3");
@ -279,7 +280,7 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) {
//
// myOptimizer.update(x);
//
// Pose2Values expected;
// DynamicValues expected;
// expected.insert(0, Pose2(0.,0.,0.));
// expected.insert(1, Pose2(1.,0.,0.));
// expected.insert(2, Pose2(2.,0.,0.));
@ -340,38 +341,38 @@ TEST(Pose2Graph, optimize2) {
using namespace pose2SLAM;
/* ************************************************************************* */
TEST( Pose2Values, pose2Circle )
TEST( DynamicValues, pose2Circle )
{
// expected is 4 poses tangent to circle with radius 1m
Pose2Values 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 ));
DynamicValues expected;
expected.insert(Key(0), Pose2( 1, 0, M_PI_2));
expected.insert(Key(1), Pose2( 0, 1, - M_PI ));
expected.insert(Key(2), Pose2(-1, 0, - M_PI_2));
expected.insert(Key(3), Pose2( 0, -1, 0 ));
Pose2Values actual = pose2SLAM::circle(4,1.0);
DynamicValues actual = pose2SLAM::circle(4,1.0);
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( Pose2Values, expmap )
TEST( DynamicValues, expmap )
{
// expected is circle shifted to right
Pose2Values 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 ));
DynamicValues expected;
expected.insert(Key(0), Pose2( 1.1, 0, M_PI_2));
expected.insert(Key(1), Pose2( 0.1, 1, - M_PI ));
expected.insert(Key(2), Pose2(-0.9, 0, - M_PI_2));
expected.insert(Key(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));
DynamicValues 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);
DynamicValues actual = circle.retract(delta, ordering);
CHECK(assert_equal(expected,actual));
}
@ -384,8 +385,8 @@ TEST( Pose2Prior, error )
{
// Choose a linearization point
Pose2 p1(1, 0, 0); // robot at (1,0)
Pose2Values x0;
x0.insert(1, p1);
DynamicValues x0;
x0.insert(Key(1), p1);
// Create factor
Pose2Prior factor(1, p1, sigmas);
@ -406,7 +407,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);
DynamicValues 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)));
@ -428,8 +429,8 @@ LieVector hprior(const Pose2& p1) {
TEST( Pose2Prior, linearize )
{
// Choose a linearization point at ground truth
Pose2Values x0;
x0.insert(1,priorVal);
DynamicValues x0;
x0.insert(Key(1),priorVal);
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
@ -448,9 +449,9 @@ TEST( Pose2Factor, error )
// Choose a linearization point
Pose2 p1; // robot at origin
Pose2 p2(1, 0, 0); // robot at (1,0)
Pose2Values x0;
x0.insert(1, p1);
x0.insert(2, p2);
DynamicValues x0;
x0.insert(Key(1), p1);
x0.insert(Key(2), p2);
// Create factor
Pose2 z = p1.between(p2);
@ -472,7 +473,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);
DynamicValues 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,9 +490,9 @@ 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;
x0.insert(1,p1);
x0.insert(2,p2);
DynamicValues x0;
x0.insert(Key(1),p1);
x0.insert(Key(2),p2);
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
@ -519,9 +520,9 @@ 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;
x0.insert(1,p1);
x0.insert(2,p2);
DynamicValues x0;
x0.insert(Key(1),p1);
x0.insert(Key(2),p2);
// expected linearization
Matrix expectedH1 = covariance->Whiten(Matrix_(3,3,

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

@ -36,6 +36,7 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
using namespace pose3SLAM;
// common measurement covariance
static Matrix covariance = eye(6);
@ -48,8 +49,8 @@ TEST(Pose3Graph, optimizeCircle) {
// Create a hexagon of poses
double radius = 10;
Pose3Values hexagon = pose3SLAM::circle(6,radius);
Pose3 gT0 = hexagon[0], gT1 = hexagon[1];
DynamicValues hexagon = pose3SLAM::circle(6,radius);
Pose3 gT0 = hexagon[Key(0)], gT1 = hexagon[Key(1)];
// create a Pose graph with one equality constraint and one measurement
shared_ptr<Pose3Graph> fg(new Pose3Graph);
@ -65,13 +66,13 @@ TEST(Pose3Graph, optimizeCircle) {
fg->addConstraint(5,0, _0T1, covariance);
// Create initial config
boost::shared_ptr<Pose3Values> initial(new Pose3Values());
initial->insert(0, gT0);
initial->insert(1, hexagon[1].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(2, hexagon[2].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(3, hexagon[3].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(4, hexagon[4].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(5, hexagon[5].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
boost::shared_ptr<DynamicValues> initial(new DynamicValues());
initial->insert(Key(0), gT0);
initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(Key(3), hexagon[Key(3)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(Key(4), hexagon[Key(4)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(Key(5), hexagon[Key(5)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
// Choose an ordering and optimize
shared_ptr<Ordering> ordering(new Ordering);
@ -80,18 +81,18 @@ TEST(Pose3Graph, optimizeCircle) {
pose3SLAM::Optimizer optimizer0(fg, initial, ordering, params);
pose3SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
Pose3Values actual = *optimizer.values();
DynamicValues actual = *optimizer.values();
// Check with ground truth
CHECK(assert_equal(hexagon, actual,1e-4));
// Check loop closure
CHECK(assert_equal(_0T1,actual[5].between(actual[0]),1e-5));
CHECK(assert_equal(_0T1,actual[Key(5)].between(actual[Key(0)]),1e-5));
}
/* ************************************************************************* */
TEST(Pose3Graph, partial_prior_height) {
typedef PartialPriorFactor<pose3SLAM::Values, pose3SLAM::Key> Partial;
typedef PartialPriorFactor<pose3SLAM::Key> Partial;
// reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3)
@ -109,13 +110,13 @@ TEST(Pose3Graph, partial_prior_height) {
pose3SLAM::Graph graph;
graph.add(height);
pose3SLAM::Values values;
DynamicValues values;
values.insert(key, init);
// linearization
EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol);
pose3SLAM::Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
DynamicValues actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
EXPECT(assert_equal(expected, actual[key], tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
}
@ -133,9 +134,9 @@ TEST( Pose3Factor, error )
Pose3Factor factor(1,2, z, I6);
// Create config
Pose3Values x;
x.insert(1,t1);
x.insert(2,t2);
DynamicValues x;
x.insert(Key(1),t1);
x.insert(Key(2),t2);
// Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2))
Vector actual = factor.unwhitenedError(x);
@ -145,7 +146,7 @@ TEST( Pose3Factor, error )
/* ************************************************************************* */
TEST(Pose3Graph, partial_prior_xy) {
typedef PartialPriorFactor<pose3SLAM::Values, pose3SLAM::Key> Partial;
typedef PartialPriorFactor<pose3SLAM::Key> Partial;
// XY prior - full mask interface
pose3SLAM::Key key(1);
@ -164,10 +165,10 @@ TEST(Pose3Graph, partial_prior_xy) {
pose3SLAM::Graph graph;
graph.add(priorXY);
pose3SLAM::Values values;
DynamicValues values;
values.insert(key, init);
pose3SLAM::Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
DynamicValues actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
EXPECT(assert_equal(expected, actual[key], tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
}
@ -180,27 +181,27 @@ 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( Pose3Values, pose3Circle )
TEST( DynamicValues, pose3Circle )
{
// expected is 4 poses tangent to circle with radius 1m
Pose3Values 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)));
DynamicValues expected;
expected.insert(Key(0), Pose3(R1, Point3( 1, 0, 0)));
expected.insert(Key(1), Pose3(R2, Point3( 0, 1, 0)));
expected.insert(Key(2), Pose3(R3, Point3(-1, 0, 0)));
expected.insert(Key(3), Pose3(R4, Point3( 0,-1, 0)));
Pose3Values actual = pose3SLAM::circle(4,1.0);
DynamicValues actual = pose3SLAM::circle(4,1.0);
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( Pose3Values, expmap )
TEST( DynamicValues, expmap )
{
Pose3Values 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)));
DynamicValues expected;
expected.insert(Key(0), Pose3(R1, Point3( 1.0, 0.1, 0)));
expected.insert(Key(1), Pose3(R2, Point3(-0.1, 1.0, 0)));
expected.insert(Key(2), Pose3(R3, Point3(-1.0,-0.1, 0)));
expected.insert(Key(3), Pose3(R4, Point3( 0.1,-1.0, 0)));
Ordering ordering(*expected.orderingArbitrary());
VectorValues delta(expected.dims(ordering));
@ -209,7 +210,7 @@ TEST( Pose3Values, expmap )
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);
Pose3Values actual = pose3SLAM::circle(4,1.0).retract(delta, ordering);
DynamicValues actual = pose3SLAM::circle(4,1.0).retract(delta, ordering);
CHECK(assert_equal(expected,actual));
}

15
gtsam/slam/tests/testProjectionFactor.cpp
View File

@ -25,6 +25,7 @@
using namespace std;
using namespace gtsam;
using namespace visualSLAM;
// make cube
static Point3
@ -51,9 +52,9 @@ TEST( ProjectionFactor, error )
factor(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
// For the following values structure, the factor predicts 320,240
visualSLAM::Values config;
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1);
Point3 l1; config.insert(1, l1);
DynamicValues config;
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(PoseKey(1), x1);
Point3 l1; config.insert(PointKey(1), l1);
// Point should project to Point2(320.,240.)
CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(config)));
@ -80,13 +81,13 @@ TEST( ProjectionFactor, error )
CHECK(assert_equal(expected_lfg,*actual_lfg));
// expmap on a config
visualSLAM::Values expected_config;
Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(1, x2);
Point3 l2(1,2,3); expected_config.insert(1, l2);
DynamicValues expected_config;
Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(PoseKey(1), x2);
Point3 l2(1,2,3); expected_config.insert(PointKey(1), l2);
VectorValues delta(expected_config.dims(ordering));
delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.);
delta[ordering["l1"]] = Vector_(3, 1.,2.,3.);
visualSLAM::Values actual_config = config.retract(delta, ordering);
DynamicValues actual_config = config.retract(delta, ordering);
CHECK(assert_equal(expected_config,actual_config,1e-9));
}

2
gtsam/slam/tests/testSimulated3D.cpp
View File

@ -28,7 +28,7 @@ using namespace gtsam;
/* ************************************************************************* */
TEST( simulated3D, Values )
{
simulated3D::Values actual;
DynamicValues actual;
actual.insert(simulated3D::PointKey(1),Point3(1,1,1));
actual.insert(simulated3D::PoseKey(2),Point3(2,2,2));
EXPECT(assert_equal(actual,actual,1e-9));

9
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -30,6 +30,7 @@
using namespace std;
using namespace gtsam;
using namespace boost;
using namespace visualSLAM;
Pose3 camera1(Matrix_(3,3,
1., 0., 0.,
@ -59,15 +60,15 @@ TEST( StereoFactor, singlePoint)
graph->add(visualSLAM::StereoFactor(z14,sigma, 1, 1, K));
// Create a configuration corresponding to the ground truth
boost::shared_ptr<visualSLAM::Values> values(new visualSLAM::Values());
values->insert(1, camera1); // add camera at z=6.25m looking towards origin
boost::shared_ptr<DynamicValues> values(new DynamicValues());
values->insert(PoseKey(1), camera1); // add camera at z=6.25m looking towards origin
Point3 l1(0, 0, 0);
values->insert(1, l1); // add point at origin;
values->insert(PointKey(1), l1); // add point at origin;
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
typedef gtsam::NonlinearOptimizer<visualSLAM::Graph,visualSLAM::Values,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> Optimizer; // optimization engine for this domain
typedef gtsam::NonlinearOptimizer<visualSLAM::Graph, gtsam::GaussianFactorGraph, gtsam::GaussianMultifrontalSolver> Optimizer; // optimization engine for this domain
double absoluteThreshold = 1e-9;
double relativeThreshold = 1e-5;

61
gtsam/slam/tests/testVSLAM.cpp
View File

@ -33,6 +33,7 @@ using namespace boost;
using namespace std;
using namespace gtsam;
using namespace boost;
using namespace visualSLAM;
static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
@ -91,13 +92,13 @@ TEST( Graph, optimizeLM)
graph->addPointConstraint(3, landmark3);
// Create an initial values structure corresponding to the ground truth
boost::shared_ptr<visualSLAM::Values> initialEstimate(new visualSLAM::Values);
initialEstimate->insert(1, camera1);
initialEstimate->insert(2, camera2);
initialEstimate->insert(1, landmark1);
initialEstimate->insert(2, landmark2);
initialEstimate->insert(3, landmark3);
initialEstimate->insert(4, landmark4);
boost::shared_ptr<DynamicValues> initialEstimate(new DynamicValues);
initialEstimate->insert(PoseKey(1), camera1);
initialEstimate->insert(PoseKey(2), camera2);
initialEstimate->insert(PointKey(1), landmark1);
initialEstimate->insert(PointKey(2), landmark2);
initialEstimate->insert(PointKey(3), landmark3);
initialEstimate->insert(PointKey(4), landmark4);
// Create an ordering of the variables
shared_ptr<Ordering> ordering(new Ordering);
@ -128,13 +129,13 @@ TEST( Graph, optimizeLM2)
graph->addPoseConstraint(2, camera2);
// Create an initial values structure corresponding to the ground truth
boost::shared_ptr<visualSLAM::Values> initialEstimate(new visualSLAM::Values);
initialEstimate->insert(1, camera1);
initialEstimate->insert(2, camera2);
initialEstimate->insert(1, landmark1);
initialEstimate->insert(2, landmark2);
initialEstimate->insert(3, landmark3);
initialEstimate->insert(4, landmark4);
boost::shared_ptr<DynamicValues> initialEstimate(new DynamicValues);
initialEstimate->insert(PoseKey(1), camera1);
initialEstimate->insert(PoseKey(2), camera2);
initialEstimate->insert(PointKey(1), landmark1);
initialEstimate->insert(PointKey(2), landmark2);
initialEstimate->insert(PointKey(3), landmark3);
initialEstimate->insert(PointKey(4), landmark4);
// Create an ordering of the variables
shared_ptr<Ordering> ordering(new Ordering);
@ -165,13 +166,13 @@ TEST( Graph, CHECK_ORDERING)
graph->addPoseConstraint(2, camera2);
// Create an initial values structure corresponding to the ground truth
boost::shared_ptr<visualSLAM::Values> initialEstimate(new visualSLAM::Values);
initialEstimate->insert(1, camera1);
initialEstimate->insert(2, camera2);
initialEstimate->insert(1, landmark1);
initialEstimate->insert(2, landmark2);
initialEstimate->insert(3, landmark3);
initialEstimate->insert(4, landmark4);
boost::shared_ptr<DynamicValues> initialEstimate(new DynamicValues);
initialEstimate->insert(PoseKey(1), camera1);
initialEstimate->insert(PoseKey(2), camera2);
initialEstimate->insert(PointKey(1), landmark1);
initialEstimate->insert(PointKey(2), landmark2);
initialEstimate->insert(PointKey(3), landmark3);
initialEstimate->insert(PointKey(4), landmark4);
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initialEstimate);
@ -193,23 +194,23 @@ TEST( Graph, CHECK_ORDERING)
TEST( Values, 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
visualSLAM::Values init;
init.insert(1, Pose3());
init.insert(1, Point3(1.0, 2.0, 3.0));
DynamicValues init;
init.insert(PoseKey(1), Pose3());
init.insert(PointKey(1), Point3(1.0, 2.0, 3.0));
visualSLAM::Values expected;
expected.insert(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
expected.insert(1, Point3(1.1, 2.1, 3.1));
DynamicValues expected;
expected.insert(PoseKey(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
expected.insert(PointKey(1), Point3(1.1, 2.1, 3.1));
Ordering largeOrdering;
visualSLAM::Values largeValues = init;
largeValues.insert(2, Pose3());
DynamicValues largeValues = init;
largeValues.insert(PoseKey(2), Pose3());
largeOrdering += "x1","l1","x2";
VectorValues delta(largeValues.dims(largeOrdering));
delta[largeOrdering["x1"]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1);
delta[largeOrdering["l1"]] = Vector_(3, 0.1, 0.1, 0.1);
delta[largeOrdering["x2"]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1);
visualSLAM::Values actual = init.retract(delta, largeOrdering);
DynamicValues actual = init.retract(delta, largeOrdering);
CHECK(assert_equal(expected,actual));
}

2
tests/Makefile.am
View File

@ -16,7 +16,7 @@ check_PROGRAMS += testGaussianJunctionTree
check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph
check_PROGRAMS += testNonlinearOptimizer testDoglegOptimizer
check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph
check_PROGRAMS += testTupleValues
#check_PROGRAMS += testTupleValues
check_PROGRAMS += testNonlinearISAM
check_PROGRAMS += testBoundingConstraint
check_PROGRAMS += testPose2SLAMwSPCG

8
tests/testBoundingConstraint.cpp
View File

@ -31,10 +31,10 @@ SharedDiagonal soft_model2 = noiseModel::Unit::Create(2);
SharedDiagonal soft_model2_alt = noiseModel::Isotropic::Sigma(2, 0.1);
SharedDiagonal hard_model1 = noiseModel::Constrained::All(1);
typedef NonlinearFactorGraph<simulated2D::Values> Graph;
typedef NonlinearFactorGraph Graph;
typedef boost::shared_ptr<Graph> shared_graph;
typedef boost::shared_ptr<simulated2D::Values> shared_values;
typedef NonlinearOptimizer<Graph, simulated2D::Values> Optimizer;
typedef NonlinearOptimizer<Graph> Optimizer;
// some simple inequality constraints
simulated2D::PoseKey key(1);
@ -160,7 +160,7 @@ TEST( testBoundingConstraint, unary_simple_optimization1) {
initValues->insert(x1, start_pt);
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
simulated2D::Values expected;
DynamicValues expected;
expected.insert(x1, goal_pt);
CHECK(assert_equal(expected, *actual, tol));
}
@ -182,7 +182,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
initValues->insert(x1, start_pt);
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
simulated2D::Values expected;
DynamicValues expected;
expected.insert(x1, goal_pt);
CHECK(assert_equal(expected, *actual, tol));
}

42
tests/testExtendedKalmanFilter.cpp
View File

@ -20,14 +20,12 @@
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Point2.h>
using namespace gtsam;
// Define Types for System Test
typedef TypedSymbol<Point2, 'x'> TestKey;
typedef Values<TestKey> TestValues;
/* ************************************************************************* */
TEST( ExtendedKalmanFilter, linear ) {
@ -37,7 +35,7 @@ TEST( ExtendedKalmanFilter, linear ) {
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
// Create an ExtendedKalmanFilter object
ExtendedKalmanFilter<TestValues,TestKey> ekf(x_initial, P_initial);
ExtendedKalmanFilter<TestKey> ekf(x_initial, P_initial);
// Create the TestKeys for our example
TestKey x0(0), x1(1), x2(2), x3(3);
@ -59,30 +57,30 @@ TEST( ExtendedKalmanFilter, linear ) {
// Run iteration 1
// Create motion factor
BetweenFactor<TestValues,TestKey> factor1(x0, x1, difference, Q);
BetweenFactor<TestKey> factor1(x0, x1, difference, Q);
Point2 predict1 = ekf.predict(factor1);
EXPECT(assert_equal(expected1,predict1));
// Create the measurement factor
PriorFactor<TestValues,TestKey> factor2(x1, z1, R);
PriorFactor<TestKey> factor2(x1, z1, R);
Point2 update1 = ekf.update(factor2);
EXPECT(assert_equal(expected1,update1));
// Run iteration 2
BetweenFactor<TestValues,TestKey> factor3(x1, x2, difference, Q);
BetweenFactor<TestKey> factor3(x1, x2, difference, Q);
Point2 predict2 = ekf.predict(factor3);
EXPECT(assert_equal(expected2,predict2));
PriorFactor<TestValues,TestKey> factor4(x2, z2, R);
PriorFactor<TestKey> factor4(x2, z2, R);
Point2 update2 = ekf.update(factor4);
EXPECT(assert_equal(expected2,update2));
// Run iteration 3
BetweenFactor<TestValues,TestKey> factor5(x2, x3, difference, Q);
BetweenFactor<TestKey> factor5(x2, x3, difference, Q);
Point2 predict3 = ekf.predict(factor5);
EXPECT(assert_equal(expected3,predict3));
PriorFactor<TestValues,TestKey> factor6(x3, z3, R);
PriorFactor<TestKey> factor6(x3, z3, R);
Point2 update3 = ekf.update(factor6);
EXPECT(assert_equal(expected3,update3));
@ -91,12 +89,12 @@ TEST( ExtendedKalmanFilter, linear ) {
// Create Motion Model Factor
class NonlinearMotionModel : public NonlinearFactor2<TestValues,TestKey,TestKey> {
class NonlinearMotionModel : public NonlinearFactor2<TestKey,TestKey> {
public:
typedef TestKey::Value T;
private:
typedef NonlinearFactor2<TestValues,TestKey,TestKey> Base;
typedef NonlinearFactor2<TestKey,TestKey> Base;
typedef NonlinearMotionModel This;
protected:
@ -163,7 +161,7 @@ public:
}
/** Check if two factors are equal. Note type is IndexFactor and needs cast. */
virtual bool equals(const NonlinearFactor2<TestValues,TestKey,TestKey>& f, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor2<TestKey,TestKey>& f, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*> (&f);
return (e != NULL) && (key1_ == e->key1_) && (key2_ == e->key2_);
}
@ -172,7 +170,7 @@ public:
* calculate the error of the factor
* Override for systems with unusual noise models
*/
virtual double error(const TestValues& c) const {
virtual double error(const DynamicValues& c) const {
Vector w = whitenedError(c);
return 0.5 * w.dot(w);
}
@ -183,7 +181,7 @@ public:
}
/** Vector of errors, whitened ! */
Vector whitenedError(const TestValues& c) const {
Vector whitenedError(const DynamicValues& c) const {
return QInvSqrt(c[key1_])*unwhitenedError(c);
}
@ -192,7 +190,7 @@ public:
* Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z
* Hence b = z - h(x1,x2) = - error_vector(x)
*/
boost::shared_ptr<GaussianFactor> linearize(const TestValues& c, const Ordering& ordering) const {
boost::shared_ptr<GaussianFactor> linearize(const DynamicValues& c, const Ordering& ordering) const {
const X1& x1 = c[key1_];
const X2& x2 = c[key2_];
Matrix A1, A2;
@ -238,13 +236,13 @@ public:
};
// Create Measurement Model Factor
class NonlinearMeasurementModel : public NonlinearFactor1<TestValues,TestKey> {
class NonlinearMeasurementModel : public NonlinearFactor1<TestKey> {
public:
typedef TestKey::Value T;
private:
typedef NonlinearFactor1<TestValues,TestKey> Base;
typedef NonlinearFactor1<TestKey> Base;
typedef NonlinearMeasurementModel This;
protected:
@ -300,7 +298,7 @@ public:
}
/** Check if two factors are equal. Note type is IndexFactor and needs cast. */
virtual bool equals(const NonlinearFactor1<TestValues,TestKey>& f, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor1<TestKey>& f, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*> (&f);
return (e != NULL) && (key_ == e->key_);
}
@ -309,7 +307,7 @@ public:
* calculate the error of the factor
* Override for systems with unusual noise models
*/
virtual double error(const TestValues& c) const {
virtual double error(const DynamicValues& c) const {
Vector w = whitenedError(c);
return 0.5 * w.dot(w);
}
@ -320,7 +318,7 @@ public:
}
/** Vector of errors, whitened ! */
Vector whitenedError(const TestValues& c) const {
Vector whitenedError(const DynamicValues& c) const {
return RInvSqrt(c[key_])*unwhitenedError(c);
}
@ -329,7 +327,7 @@ public:
* Ax-b \approx h(x1+dx1)-z = h(x1) + A1*dx1 - z
* Hence b = z - h(x1) = - error_vector(x)
*/
boost::shared_ptr<GaussianFactor> linearize(const TestValues& c, const Ordering& ordering) const {
boost::shared_ptr<GaussianFactor> linearize(const DynamicValues& c, const Ordering& ordering) const {
const X& x1 = c[key_];
Matrix A1;
Vector b = -evaluateError(x1, A1);
@ -395,7 +393,7 @@ TEST( ExtendedKalmanFilter, nonlinear ) {
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
// Create an ExtendedKalmanFilter object
ExtendedKalmanFilter<TestValues,TestKey> ekf(x_initial, P_initial);
ExtendedKalmanFilter<TestKey> ekf(x_initial, P_initial);
// Enter Predict-Update Loop
Point2 x_predict, x_update;

110
tests/testGaussianISAM2.cpp
View File

@ -33,10 +33,10 @@ const double tol = 1e-4;
TEST(ISAM2, AddVariables) {
// Create initial state
planarSLAM::Values theta;
DynamicValues theta;
theta.insert(planarSLAM::PoseKey(0), Pose2(.1, .2, .3));
theta.insert(planarSLAM::PointKey(0), Point2(.4, .5));
planarSLAM::Values newTheta;
DynamicValues newTheta;
newTheta.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8));
VectorValues deltaUnpermuted;
@ -51,7 +51,7 @@ TEST(ISAM2, AddVariables) {
Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0);
GaussianISAM2<planarSLAM::Values>::Nodes nodes(2);
GaussianISAM2<>::Nodes nodes(2);
// Verify initial state
LONGS_EQUAL(0, ordering[planarSLAM::PointKey(0)]);
@ -60,7 +60,7 @@ TEST(ISAM2, AddVariables) {
EXPECT(assert_equal(deltaUnpermuted[0], delta[ordering[planarSLAM::PoseKey(0)]]));
// Create expected state
planarSLAM::Values thetaExpected;
DynamicValues thetaExpected;
thetaExpected.insert(planarSLAM::PoseKey(0), Pose2(.1, .2, .3));
thetaExpected.insert(planarSLAM::PointKey(0), Point2(.4, .5));
thetaExpected.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8));
@ -79,11 +79,11 @@ TEST(ISAM2, AddVariables) {
Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1);
GaussianISAM2<planarSLAM::Values>::Nodes nodesExpected(
3, GaussianISAM2<planarSLAM::Values>::sharedClique());
GaussianISAM2<>::Nodes nodesExpected(
3, GaussianISAM2<>::sharedClique());
// Expand initial state
GaussianISAM2<planarSLAM::Values>::Impl::AddVariables(newTheta, theta, delta, ordering, nodes);
GaussianISAM2<>::Impl::AddVariables(newTheta, theta, delta, ordering, nodes);
EXPECT(assert_equal(thetaExpected, theta));
EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted));
@ -148,7 +148,7 @@ TEST(ISAM2, AddVariables) {
TEST(ISAM2, optimize2) {
// Create initialization
planarSLAM::Values theta;
DynamicValues theta;
theta.insert(planarSLAM::PoseKey(0), Pose2(0.01, 0.01, 0.01));
// Create conditional
@ -168,8 +168,8 @@ TEST(ISAM2, optimize2) {
conditional->solveInPlace(expected);
// Clique
GaussianISAM2<planarSLAM::Values>::sharedClique clique(
GaussianISAM2<planarSLAM::Values>::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr())));
GaussianISAM2<>::sharedClique clique(
GaussianISAM2<>::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr())));
VectorValues actual(theta.dims(ordering));
conditional->rhs(actual);
optimize2(clique, actual);
@ -180,15 +180,15 @@ TEST(ISAM2, optimize2) {
}
/* ************************************************************************* */
bool isam_check(const planarSLAM::Graph& fullgraph, const planarSLAM::Values& fullinit, const GaussianISAM2<planarSLAM::Values>& isam) {
planarSLAM::Values actual = isam.calculateEstimate();
bool isam_check(const planarSLAM::Graph& fullgraph, const DynamicValues& fullinit, const GaussianISAM2<>& isam) {
DynamicValues actual = isam.calculateEstimate();
Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first;
GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering);
// linearized.print("Expected linearized: ");
GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate();
// gbn.print("Expected bayesnet: ");
VectorValues delta = optimize(gbn);
planarSLAM::Values expected = fullinit.retract(delta, ordering);
DynamicValues expected = fullinit.retract(delta, ordering);
return assert_equal(expected, actual);
}
@ -210,8 +210,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
planarSLAM::Values fullinit;
GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
DynamicValues fullinit;
planarSLAM::Graph fullgraph;
// i keeps track of the time step
@ -223,7 +223,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
@ -238,7 +238,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
@ -253,7 +253,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
@ -271,7 +271,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
@ -286,7 +286,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
@ -298,7 +298,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
CHECK(isam_check(fullgraph, fullinit, isam));
// Check gradient at each node
typedef GaussianISAM2<planarSLAM::Values>::sharedClique sharedClique;
typedef GaussianISAM2<>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient
FactorGraph<JacobianFactor> jfg;
@ -343,8 +343,8 @@ TEST(ISAM2, slamlike_solution_dogleg)
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
planarSLAM::Values fullinit;
GaussianISAM2<> isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
DynamicValues fullinit;
planarSLAM::Graph fullgraph;
// i keeps track of the time step
@ -356,7 +356,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
@ -371,7 +371,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
@ -386,7 +386,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
@ -404,7 +404,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
@ -419,7 +419,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
@ -431,7 +431,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
CHECK(isam_check(fullgraph, fullinit, isam));
// Check gradient at each node
typedef GaussianISAM2<planarSLAM::Values>::sharedClique sharedClique;
typedef GaussianISAM2<>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient
FactorGraph<JacobianFactor> jfg;
@ -471,8 +471,8 @@ TEST(ISAM2, clone) {
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
planarSLAM::Values fullinit;
GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
DynamicValues fullinit;
planarSLAM::Graph fullgraph;
// i keeps track of the time step
@ -484,7 +484,7 @@ TEST(ISAM2, clone) {
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
@ -499,7 +499,7 @@ TEST(ISAM2, clone) {
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
@ -514,7 +514,7 @@ TEST(ISAM2, clone) {
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
@ -532,7 +532,7 @@ TEST(ISAM2, clone) {
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
@ -547,7 +547,7 @@ TEST(ISAM2, clone) {
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
@ -556,8 +556,8 @@ TEST(ISAM2, clone) {
}
// CLONING...
boost::shared_ptr<GaussianISAM2<planarSLAM::Values> > isam2
= boost::shared_ptr<GaussianISAM2<planarSLAM::Values> >(new GaussianISAM2<planarSLAM::Values>());
boost::shared_ptr<GaussianISAM2<> > isam2
= boost::shared_ptr<GaussianISAM2<> >(new GaussianISAM2<>());
isam.cloneTo(isam2);
CHECK(assert_equal(isam, *isam2));
@ -644,8 +644,8 @@ TEST(ISAM2, removeFactors)
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
planarSLAM::Values fullinit;
GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
DynamicValues fullinit;
planarSLAM::Graph fullgraph;
// i keeps track of the time step
@ -657,7 +657,7 @@ TEST(ISAM2, removeFactors)
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
@ -672,7 +672,7 @@ TEST(ISAM2, removeFactors)
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
@ -687,7 +687,7 @@ TEST(ISAM2, removeFactors)
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
@ -705,7 +705,7 @@ TEST(ISAM2, removeFactors)
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
@ -721,7 +721,7 @@ TEST(ISAM2, removeFactors)
fullgraph.push_back(newfactors[0]);
fullgraph.push_back(newfactors[2]); // Don't add measurement on landmark 0
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
@ -731,14 +731,14 @@ TEST(ISAM2, removeFactors)
// Remove the measurement on landmark 0
FastVector<size_t> toRemove;
toRemove.push_back(result.newFactorsIndices[1]);
isam.update(planarSLAM::Graph(), planarSLAM::Values(), toRemove);
isam.update(planarSLAM::Graph(), DynamicValues(), toRemove);
}
// Compare solutions
CHECK(isam_check(fullgraph, fullinit, isam));
// Check gradient at each node
typedef GaussianISAM2<planarSLAM::Values>::sharedClique sharedClique;
typedef GaussianISAM2<>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient
FactorGraph<JacobianFactor> jfg;
@ -783,8 +783,8 @@ TEST(ISAM2, constrained_ordering)
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
planarSLAM::Values fullinit;
GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
DynamicValues fullinit;
planarSLAM::Graph fullgraph;
// We will constrain x3 and x4 to the end
@ -799,7 +799,7 @@ TEST(ISAM2, constrained_ordering)
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
@ -814,7 +814,7 @@ TEST(ISAM2, constrained_ordering)
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
@ -832,7 +832,7 @@ TEST(ISAM2, constrained_ordering)
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
@ -850,7 +850,7 @@ TEST(ISAM2, constrained_ordering)
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
@ -865,7 +865,7 @@ TEST(ISAM2, constrained_ordering)
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
planarSLAM::Values init;
DynamicValues init;
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
@ -881,7 +881,7 @@ TEST(ISAM2, constrained_ordering)
(isam.getOrdering()[planarSLAM::PoseKey(3)] == 13 && isam.getOrdering()[planarSLAM::PoseKey(4)] == 12));
// Check gradient at each node
typedef GaussianISAM2<planarSLAM::Values>::sharedClique sharedClique;
typedef GaussianISAM2<>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient
FactorGraph<JacobianFactor> jfg;

6
tests/testGaussianJunctionTree.cpp
View File

@ -198,9 +198,9 @@ TEST(GaussianJunctionTree, simpleMarginal) {
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)));
pose2SLAM::Values init;
init.insert(0, Pose2());
init.insert(1, Pose2(1.0, 0.0, 0.0));
DynamicValues init;
init.insert(pose2SLAM::Key(0), Pose2());
init.insert(pose2SLAM::Key(1), Pose2(1.0, 0.0, 0.0));
Ordering ordering;
ordering += "x1", "x0";

24
tests/testGraph.cpp
View File

@ -86,22 +86,22 @@ TEST( Graph, composePoses )
graph.addConstraint(2,3, p23, cov);
graph.addConstraint(4,3, p43, cov);
PredecessorMap<Pose2Values::Key> tree;
tree.insert(1,2);
tree.insert(2,2);
tree.insert(3,2);
tree.insert(4,3);
PredecessorMap<pose2SLAM::Key> tree;
tree.insert(pose2SLAM::Key(1),2);
tree.insert(pose2SLAM::Key(2),2);
tree.insert(pose2SLAM::Key(3),2);
tree.insert(pose2SLAM::Key(4),3);
Pose2 rootPose = p2;
boost::shared_ptr<Pose2Values> actual = composePoses<Pose2Graph, Pose2Factor,
Pose2, Pose2Values> (graph, tree, rootPose);
boost::shared_ptr<DynamicValues> actual = composePoses<Pose2Graph, Pose2Factor,
Pose2, pose2SLAM::Key> (graph, tree, rootPose);
Pose2Values expected;
expected.insert(1, p1);
expected.insert(2, p2);
expected.insert(3, p3);
expected.insert(4, p4);
DynamicValues expected;
expected.insert(pose2SLAM::Key(1), p1);
expected.insert(pose2SLAM::Key(2), p2);
expected.insert(pose2SLAM::Key(3), p3);
expected.insert(pose2SLAM::Key(4), p4);
LONGS_EQUAL(4, actual->size());
CHECK(assert_equal(expected, *actual));

56
tests/testNonlinearEquality.cpp
View File

@ -32,20 +32,19 @@ namespace eq2D = gtsam::simulated2D::equality_constraints;
static const double tol = 1e-5;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef Values<PoseKey> PoseValues;
typedef PriorFactor<PoseValues, PoseKey> PosePrior;
typedef NonlinearEquality<PoseValues, PoseKey> PoseNLE;
typedef PriorFactor<PoseKey> PosePrior;
typedef NonlinearEquality<PoseKey> PoseNLE;
typedef boost::shared_ptr<PoseNLE> shared_poseNLE;
typedef NonlinearFactorGraph<PoseValues> PoseGraph;
typedef NonlinearOptimizer<PoseGraph,PoseValues> PoseOptimizer;
typedef NonlinearFactorGraph PoseGraph;
typedef NonlinearOptimizer<PoseGraph> PoseOptimizer;
PoseKey key(1);
/* ************************************************************************* */
TEST ( NonlinearEquality, linearization ) {
Pose2 value = Pose2(2.1, 1.0, 2.0);
PoseValues linearize;
DynamicValues linearize;
linearize.insert(key, value);
// create a nonlinear equality constraint
@ -63,7 +62,7 @@ TEST ( NonlinearEquality, linearization_pose ) {
PoseKey key(1);
Pose2 value;
PoseValues config;
DynamicValues config;
config.insert(key, value);
// create a nonlinear equality constraint
@ -77,7 +76,7 @@ TEST ( NonlinearEquality, linearization_pose ) {
TEST ( NonlinearEquality, linearization_fail ) {
Pose2 value = Pose2(2.1, 1.0, 2.0);
Pose2 wrong = Pose2(2.1, 3.0, 4.0);
PoseValues bad_linearize;
DynamicValues bad_linearize;
bad_linearize.insert(key, wrong);
// create a nonlinear equality constraint
@ -93,7 +92,7 @@ TEST ( NonlinearEquality, linearization_fail_pose ) {
PoseKey key(1);
Pose2 value(2.0, 1.0, 2.0),
wrong(2.0, 3.0, 4.0);
PoseValues bad_linearize;
DynamicValues bad_linearize;
bad_linearize.insert(key, wrong);
// create a nonlinear equality constraint
@ -109,7 +108,7 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) {
PoseKey key(1);
Pose2 value,
wrong(2.0, 3.0, 4.0);
PoseValues bad_linearize;
DynamicValues bad_linearize;
bad_linearize.insert(key, wrong);
// create a nonlinear equality constraint
@ -123,7 +122,7 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) {
TEST ( NonlinearEquality, error ) {
Pose2 value = Pose2(2.1, 1.0, 2.0);
Pose2 wrong = Pose2(2.1, 3.0, 4.0);
PoseValues feasible, bad_linearize;
DynamicValues feasible, bad_linearize;
feasible.insert(key, value);
bad_linearize.insert(key, wrong);
@ -168,7 +167,7 @@ TEST ( NonlinearEquality, allow_error_pose ) {
EXPECT(assert_equal(expVec, actVec, 1e-5));
// the actual error should have a gain on it
PoseValues config;
DynamicValues config;
config.insert(key1, badPoint1);
double actError = nle.error(config);
DOUBLES_EQUAL(500.0, actError, 1e-9);
@ -195,7 +194,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
// initialize away from the ideal
Pose2 initPose(0.0, 2.0, 3.0);
boost::shared_ptr<PoseValues> init(new PoseValues());
boost::shared_ptr<DynamicValues> init(new DynamicValues());
init->insert(key1, initPose);
// optimize
@ -206,7 +205,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
PoseOptimizer result = optimizer.levenbergMarquardt();
// verify
PoseValues expected;
DynamicValues expected;
expected.insert(key1, feasible1);
EXPECT(assert_equal(expected, *result.values()));
}
@ -219,7 +218,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
Pose2 feasible1(1.0, 2.0, 3.0);
// initialize away from the ideal
boost::shared_ptr<PoseValues> init(new PoseValues());
boost::shared_ptr<DynamicValues> init(new DynamicValues());
Pose2 initPose(0.0, 2.0, 3.0);
init->insert(key1, initPose);
@ -242,7 +241,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
PoseOptimizer result = optimizer.levenbergMarquardt();
// verify
PoseValues expected;
DynamicValues expected;
expected.insert(key1, feasible1);
EXPECT(assert_equal(expected, *result.values()));
}
@ -251,10 +250,10 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
SharedDiagonal hard_model = noiseModel::Constrained::All(2);
SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
typedef NonlinearFactorGraph<simulated2D::Values> Graph;
typedef NonlinearFactorGraph Graph;
typedef boost::shared_ptr<Graph> shared_graph;
typedef boost::shared_ptr<simulated2D::Values> shared_values;
typedef NonlinearOptimizer<Graph, simulated2D::Values> Optimizer;
typedef boost::shared_ptr<DynamicValues> shared_values;
typedef NonlinearOptimizer<Graph> Optimizer;
/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, unary_basics ) {
@ -326,7 +325,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
// verify error values
EXPECT(constraint->active(*initValues));
simulated2D::Values expected;
DynamicValues expected;
expected.insert(key, truth_pt);
EXPECT(constraint->active(expected));
EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol);
@ -423,7 +422,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
initValues->insert(key2, badPt);
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
simulated2D::Values expected;
DynamicValues expected;
expected.insert(key1, truth_pt1);
expected.insert(key2, truth_pt2);
CHECK(assert_equal(expected, *actual, tol));
@ -463,7 +462,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate);
simulated2D::Values expected;
DynamicValues expected;
expected.insert(x1, pt_x1);
expected.insert(l1, Point2(1.0, 6.0));
expected.insert(l2, Point2(1.0, 6.0));
@ -507,7 +506,7 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
// optimize
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate);
simulated2D::Values expected;
DynamicValues expected;
expected.insert(x1, Point2(1.0, 1.0));
expected.insert(l1, Point2(1.0, 6.0));
expected.insert(l2, Point2(1.0, 6.0));
@ -522,13 +521,12 @@ Cal3_S2 K(fov,w,h);
boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
// typedefs for visual SLAM example
typedef visualSLAM::Values VValues;
typedef boost::shared_ptr<VValues> shared_vconfig;
typedef boost::shared_ptr<DynamicValues> shared_vconfig;
typedef visualSLAM::Graph VGraph;
typedef NonlinearOptimizer<VGraph,VValues> VOptimizer;
typedef NonlinearOptimizer<VGraph> VOptimizer;
// factors for visual slam
typedef NonlinearEquality2<VValues, visualSLAM::PointKey> Point3Equality;
typedef NonlinearEquality2<visualSLAM::PointKey> Point3Equality;
/* ********************************************************************* */
TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
@ -567,7 +565,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
Point3 landmark1(0.5, 5.0, 0.0);
Point3 landmark2(1.5, 5.0, 0.0);
shared_vconfig initValues(new VValues());
shared_vconfig initValues(new DynamicValues());
initValues->insert(x1, pose1);
initValues->insert(x2, pose2);
initValues->insert(l1, landmark1);
@ -577,7 +575,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
VOptimizer::shared_values actual = VOptimizer::optimizeLM(graph, initValues);
// create config
VValues truthValues;
DynamicValues truthValues;
truthValues.insert(x1, camera1.pose());
truthValues.insert(x2, camera2.pose());
truthValues.insert(l1, landmark);

53
tests/testNonlinearFactor.cpp
View File

@ -40,7 +40,7 @@ using namespace std;
using namespace gtsam;
using namespace example;
typedef boost::shared_ptr<NonlinearFactor<VectorValues> > shared_nlf;
typedef boost::shared_ptr<NonlinearFactor > shared_nlf;
/* ************************************************************************* */
TEST( NonlinearFactor, equals )
@ -89,7 +89,7 @@ TEST( NonlinearFactor, NonlinearFactor )
// calculate the error_vector from the factor "f1"
// error_vector = [0.1 0.1]
Vector actual_e = boost::dynamic_pointer_cast<NoiseModelFactor<example::Values> >(factor)->unwhitenedError(cfg);
Vector actual_e = boost::dynamic_pointer_cast<NoiseModelFactor>(factor)->unwhitenedError(cfg);
CHECK(assert_equal(0.1*ones(2),actual_e));
// error = 0.5 * [1 1] * [1;1] = 1
@ -236,12 +236,11 @@ TEST( NonlinearFactor, linearize_constraint2 )
/* ************************************************************************* */
typedef TypedSymbol<LieVector, 'x'> TestKey;
typedef gtsam::Values<TestKey> TestValues;
/* ************************************************************************* */
class TestFactor4 : public NonlinearFactor4<TestValues, TestKey, TestKey, TestKey, TestKey> {
class TestFactor4 : public NonlinearFactor4<TestKey, TestKey, TestKey, TestKey> {
public:
typedef NonlinearFactor4<TestValues, TestKey, TestKey, TestKey, TestKey> Base;
typedef NonlinearFactor4<TestKey, TestKey, TestKey, TestKey> Base;
TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4) {}
virtual Vector
@ -263,11 +262,11 @@ public:
/* ************************************ */
TEST(NonlinearFactor, NonlinearFactor4) {
TestFactor4 tf;
TestValues tv;
tv.insert(1, LieVector(1, 1.0));
tv.insert(2, LieVector(1, 2.0));
tv.insert(3, LieVector(1, 3.0));
tv.insert(4, LieVector(1, 4.0));
DynamicValues tv;
tv.insert(TestKey(1), LieVector(1, 1.0));
tv.insert(TestKey(2), LieVector(1, 2.0));
tv.insert(TestKey(3), LieVector(1, 3.0));
tv.insert(TestKey(4), LieVector(1, 4.0));
EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4);
@ -284,9 +283,9 @@ TEST(NonlinearFactor, NonlinearFactor4) {
}
/* ************************************************************************* */
class TestFactor5 : public NonlinearFactor5<TestValues, TestKey, TestKey, TestKey, TestKey, TestKey> {
class TestFactor5 : public NonlinearFactor5<TestKey, TestKey, TestKey, TestKey, TestKey> {
public:
typedef NonlinearFactor5<TestValues, TestKey, TestKey, TestKey, TestKey, TestKey> Base;
typedef NonlinearFactor5<TestKey, TestKey, TestKey, TestKey, TestKey> Base;
TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5) {}
virtual Vector
@ -310,12 +309,12 @@ public:
/* ************************************ */
TEST(NonlinearFactor, NonlinearFactor5) {
TestFactor5 tf;
TestValues tv;
tv.insert(1, LieVector(1, 1.0));
tv.insert(2, LieVector(1, 2.0));
tv.insert(3, LieVector(1, 3.0));
tv.insert(4, LieVector(1, 4.0));
tv.insert(5, LieVector(1, 5.0));
DynamicValues tv;
tv.insert(TestKey(1), LieVector(1, 1.0));
tv.insert(TestKey(2), LieVector(1, 2.0));
tv.insert(TestKey(3), LieVector(1, 3.0));
tv.insert(TestKey(4), LieVector(1, 4.0));
tv.insert(TestKey(5), LieVector(1, 5.0));
EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5);
@ -334,9 +333,9 @@ TEST(NonlinearFactor, NonlinearFactor5) {
}
/* ************************************************************************* */
class TestFactor6 : public NonlinearFactor6<TestValues, TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> {
class TestFactor6 : public NonlinearFactor6<TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> {
public:
typedef NonlinearFactor6<TestValues, TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> Base;
typedef NonlinearFactor6<TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> Base;
TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5, 6) {}
virtual Vector
@ -362,13 +361,13 @@ public:
/* ************************************ */
TEST(NonlinearFactor, NonlinearFactor6) {
TestFactor6 tf;
TestValues tv;
tv.insert(1, LieVector(1, 1.0));
tv.insert(2, LieVector(1, 2.0));
tv.insert(3, LieVector(1, 3.0));
tv.insert(4, LieVector(1, 4.0));
tv.insert(5, LieVector(1, 5.0));
tv.insert(6, LieVector(1, 6.0));
DynamicValues tv;
tv.insert(TestKey(1), LieVector(1, 1.0));
tv.insert(TestKey(2), LieVector(1, 2.0));
tv.insert(TestKey(3), LieVector(1, 3.0));
tv.insert(TestKey(4), LieVector(1, 4.0));
tv.insert(TestKey(5), LieVector(1, 5.0));
tv.insert(TestKey(6), LieVector(1, 6.0));
EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5), TestKey(6);

6
tests/testNonlinearISAM.cpp
View File

@ -12,7 +12,7 @@
using namespace gtsam;
using namespace planarSLAM;
typedef NonlinearISAM<planarSLAM::Values> PlanarISAM;
typedef NonlinearISAM<> PlanarISAM;
const double tol=1e-5;
@ -30,8 +30,8 @@ TEST(testNonlinearISAM, markov_chain ) {
Graph start_factors;
start_factors.addPoseConstraint(key, cur_pose);
planarSLAM::Values init;
planarSLAM::Values expected;
DynamicValues init;
DynamicValues expected;
init.insert(key, cur_pose);
expected.insert(key, cur_pose);
isam.update(start_factors, init);

24
tests/testNonlinearOptimizer.cpp
View File

@ -44,7 +44,7 @@ using namespace gtsam;
const double tol = 1e-5;
typedef NonlinearOptimizer<example::Graph,example::Values> Optimizer;
typedef NonlinearOptimizer<example::Graph> Optimizer;
/* ************************************************************************* */
TEST( NonlinearOptimizer, linearizeAndOptimizeForDelta )
@ -241,11 +241,11 @@ TEST( NonlinearOptimizer, optimization_method )
example::Values c0;
c0.insert(simulated2D::PoseKey(1), x0);
example::Values actualMFQR = optimize<example::Graph,example::Values>(
DynamicValues actualMFQR = optimize<example::Graph>(
fg, c0, *NonlinearOptimizationParameters().newFactorization(true), MULTIFRONTAL, LM);
DOUBLES_EQUAL(0,fg.error(actualMFQR),tol);
example::Values actualMFLDL = optimize<example::Graph,example::Values>(
DynamicValues actualMFLDL = optimize<example::Graph>(
fg, c0, *NonlinearOptimizationParameters().newFactorization(false), MULTIFRONTAL, LM);
DOUBLES_EQUAL(0,fg.error(actualMFLDL),tol);
}
@ -253,26 +253,26 @@ TEST( NonlinearOptimizer, optimization_method )
/* ************************************************************************* */
TEST( NonlinearOptimizer, Factorization )
{
typedef NonlinearOptimizer<Pose2Graph, Pose2Values, GaussianFactorGraph, GaussianSequentialSolver > Optimizer;
typedef NonlinearOptimizer<Pose2Graph, GaussianFactorGraph, GaussianSequentialSolver > Optimizer;
boost::shared_ptr<Pose2Values> config(new Pose2Values);
config->insert(1, Pose2(0.,0.,0.));
config->insert(2, Pose2(1.5,0.,0.));
boost::shared_ptr<DynamicValues> config(new DynamicValues);
config->insert(pose2SLAM::Key(1), Pose2(0.,0.,0.));
config->insert(pose2SLAM::Key(2), Pose2(1.5,0.,0.));
boost::shared_ptr<Pose2Graph> graph(new Pose2Graph);
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));
boost::shared_ptr<Ordering> ordering(new Ordering);
ordering->push_back(Pose2Values::Key(1));
ordering->push_back(Pose2Values::Key(2));
ordering->push_back(pose2SLAM::Key(1));
ordering->push_back(pose2SLAM::Key(2));
Optimizer optimizer(graph, config, ordering);
Optimizer optimized = optimizer.iterateLM();
Pose2Values expected;
expected.insert(1, Pose2(0.,0.,0.));
expected.insert(2, Pose2(1.,0.,0.));
DynamicValues expected;
expected.insert(pose2SLAM::Key(1), Pose2(0.,0.,0.));
expected.insert(pose2SLAM::Key(2), Pose2(1.,0.,0.));
CHECK(assert_equal(expected, *optimized.values(), 1e-5));
}

6
tests/testPose2SLAMwSPCG.cpp
View File

@ -40,7 +40,7 @@ TEST(testPose2SLAMwSPCG, example1) {
graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ;
graph.addPrior(x1, Pose2(0,0,0), sigma) ;
pose2SLAM::Values initial;
DynamicValues initial;
initial.insert(x1, Pose2( 0, 0, 0));
initial.insert(x2, Pose2( 0, 2.1, 0.01));
initial.insert(x3, Pose2( 0, 3.9,-0.01));
@ -51,7 +51,7 @@ TEST(testPose2SLAMwSPCG, example1) {
initial.insert(x8, Pose2(3.9, 2.1, 0.01));
initial.insert(x9, Pose2(4.1, 3.9,-0.01));
pose2SLAM::Values expected;
DynamicValues expected;
expected.insert(x1, Pose2(0.0, 0.0, 0.0));
expected.insert(x2, Pose2(0.0, 2.0, 0.0));
expected.insert(x3, Pose2(0.0, 4.0, 0.0));
@ -62,7 +62,7 @@ TEST(testPose2SLAMwSPCG, example1) {
expected.insert(x8, Pose2(4.0, 2.0, 0.0));
expected.insert(x9, Pose2(4.0, 4.0, 0.0));
pose2SLAM::Values actual = optimizeSPCG(graph, initial);
DynamicValues actual = optimizeSPCG(graph, initial);
EXPECT(assert_equal(expected, actual, tol));
}

20
tests/testRot3Optimization.cpp
View File

@ -23,7 +23,6 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
@ -31,27 +30,26 @@
using namespace gtsam;
typedef TypedSymbol<Rot3, 'r'> Key;
typedef Values<Key> Rot3Values;
typedef PriorFactor<Rot3Values, Key> Prior;
typedef BetweenFactor<Rot3Values, Key> Between;
typedef NonlinearFactorGraph<Rot3Values> Graph;
typedef PriorFactor<Key> Prior;
typedef BetweenFactor<Key> Between;
typedef NonlinearFactorGraph Graph;
/* ************************************************************************* */
TEST(Rot3, optimize) {
// Optimize a circle
Rot3Values truth;
Rot3Values initial;
DynamicValues truth;
DynamicValues initial;
Graph fg;
fg.add(Prior(0, Rot3(), sharedSigma(3, 0.01)));
for(int j=0; j<6; ++j) {
truth.insert(j, Rot3::Rz(M_PI/3.0 * double(j)));
initial.insert(j, Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
fg.add(Between(j, (j+1)%6, Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
truth.insert(Key(j), Rot3::Rz(M_PI/3.0 * double(j)));
initial.insert(Key(j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
fg.add(Between(Key(j), Key((j+1)%6), Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
}
NonlinearOptimizationParameters params;
Rot3Values final = optimize(fg, initial, params);
DynamicValues final = optimize(fg, initial, params);
EXPECT(assert_equal(truth, final, 1e-5));
}