diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index 5692d9a92..5fc784a3e 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -139,19 +139,19 @@ predecessorMap2Graph(const PredecessorMap& p_map) { } /* ************************************************************************* */ -template +template class compose_key_visitor : public boost::default_bfs_visitor { private: - boost::shared_ptr config_; + boost::shared_ptr config_; public: - compose_key_visitor(boost::shared_ptr config_in) {config_ = config_in;} + compose_key_visitor(boost::shared_ptr config_in) {config_ = config_in;} template 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 -boost::shared_ptr composePoses(const G& graph, const PredecessorMap& tree, +template +boost::shared_ptr composePoses(const G& graph, const PredecessorMap& 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::property, boost::property > PoseGraph; typedef typename boost::graph_traits::vertex_descriptor PoseVertex; typedef typename boost::graph_traits::edge_descriptor PoseEdge; PoseGraph g; PoseVertex root; - map key2vertex; + map key2vertex; boost::tie(g, root, key2vertex) = - predecessorMap2Graph(tree); + predecessorMap2Graph(tree); // attach the relative poses to the edges PoseEdge edge12, edge21; @@ -189,8 +189,8 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap factor = boost::dynamic_pointer_cast(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 composePoses(const G& graph, const PredecessorMap config(new VALUES); - typename VALUES::Key rootKey = boost::get(boost::vertex_name, g, root); + boost::shared_ptr config(new DynamicValues); + KEY rootKey = boost::get(boost::vertex_name, g, root); config->insert(rootKey, rootPose); - compose_key_visitor vis(config); + compose_key_visitor vis(config); boost::breadth_first_search(g, root, boost::visitor(vis)); return config; diff --git a/gtsam/inference/graph.h b/gtsam/inference/graph.h index 7a34159ca..6e51a9f6c 100644 --- a/gtsam/inference/graph.h +++ b/gtsam/inference/graph.h @@ -25,6 +25,7 @@ #include #include #include +#include namespace gtsam { @@ -87,9 +88,9 @@ namespace gtsam { /** * Compose the poses by following the chain specified by the spanning tree */ - template - boost::shared_ptr - composePoses(const G& graph, const PredecessorMap& tree, const POSE& rootPose); + template + boost::shared_ptr + composePoses(const G& graph, const PredecessorMap& tree, const POSE& rootPose); /** diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index f641ab9e7..42eb2e795 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -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 diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 09d54d524..707dc3350 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -114,15 +114,15 @@ namespace gtsam { * This constraint requires the underlying type to a Lie type * */ - template - class BetweenConstraint : public BetweenFactor { + template + class BetweenConstraint : public BetweenFactor { public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; /** Syntactic sugar for constrained version */ BetweenConstraint(const typename KEY::Value& measured, const KEY& key1, const KEY& key2, double mu = 1000.0) - : BetweenFactor(key1, key2, measured, + : BetweenFactor(key1, key2, measured, noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {} private: @@ -132,7 +132,7 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("BetweenFactor", - boost::serialization::base_object >(*this)); + boost::serialization::base_object >(*this)); } }; // \class BetweenConstraint diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 66ec820c4..043999b15 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -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 + template class GeneralSFMFactor: - public NonlinearFactor2 { + public NonlinearFactor2 { protected: Point2 z_; ///< the 2D measurement public: typedef typename CamK::Value Cam; ///< typedef for camera type - typedef GeneralSFMFactor Self ; ///< typedef for this object - typedef NonlinearFactor2 Base; ///< typedef for the base class + typedef GeneralSFMFactor Self ; ///< typedef for this object + typedef NonlinearFactor2 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 > shared_ptr; + typedef boost::shared_ptr > shared_ptr; /** * Constructor @@ -71,7 +71,7 @@ namespace gtsam { /** * equals */ - bool equals(const GeneralSFMFactor &p, double tol = 1e-9) const { + bool equals(const GeneralSFMFactor &p, double tol = 1e-9) const { return Base::equals(p, tol) && this->z_.equals(p.z_, tol) ; } diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am index fa51a9512..43e5c970e 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -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 diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h index 4d66620dd..651e55500 100644 --- a/gtsam/slam/PartialPriorFactor.h +++ b/gtsam/slam/PartialPriorFactor.h @@ -95,7 +95,7 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) && diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h index b09a5c638..ca7a5da6c 100644 --- a/gtsam/slam/simulated2DConstraints.h +++ b/gtsam/slam/simulated2DConstraints.h @@ -35,13 +35,13 @@ namespace gtsam { namespace equality_constraints { /** Typedefs for regular use */ - typedef NonlinearEquality1 UnaryEqualityConstraint; - typedef NonlinearEquality1 UnaryEqualityPointConstraint; - typedef BetweenConstraint OdoEqualityConstraint; + typedef NonlinearEquality1 UnaryEqualityConstraint; + typedef NonlinearEquality1 UnaryEqualityPointConstraint; + typedef BetweenConstraint OdoEqualityConstraint; /** Equality between variables */ - typedef NonlinearEquality2 PoseEqualityConstraint; - typedef NonlinearEquality2 PointEqualityConstraint; + typedef NonlinearEquality2 PoseEqualityConstraint; + typedef NonlinearEquality2 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 - struct ScalarCoordConstraint1: public BoundingConstraint1 { - typedef BoundingConstraint1 Base; ///< Base class convenience typedef - typedef boost::shared_ptr > shared_ptr; ///< boost::shared_ptr convenience typedef + template + struct ScalarCoordConstraint1: public BoundingConstraint1 { + typedef BoundingConstraint1 Base; ///< Base class convenience typedef + typedef boost::shared_ptr > 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 PoseXInequality; ///< Simulated2D domain example factor constraining X - typedef ScalarCoordConstraint1 PoseYInequality; ///< Simulated2D domain example factor constraining Y + typedef ScalarCoordConstraint1 PoseXInequality; ///< Simulated2D domain example factor constraining X + typedef ScalarCoordConstraint1 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 - struct MaxDistanceConstraint : public BoundingConstraint2 { - typedef BoundingConstraint2 Base; ///< Base class for factor + template + struct MaxDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 Base; ///< Base class for factor typedef typename KEY::Value Point; ///< Type of variable constrained virtual ~MaxDistanceConstraint() {} @@ -150,7 +150,7 @@ namespace gtsam { } }; - typedef MaxDistanceConstraint PoseMaxDistConstraint; ///< Simulated2D domain example factor + typedef MaxDistanceConstraint 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 - struct MinDistanceConstraint : public BoundingConstraint2 { - typedef BoundingConstraint2 Base; ///< Base class for factor + template + struct MinDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 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 LandmarkAvoid; ///< Simulated2D domain example factor + typedef MinDistanceConstraint LandmarkAvoid; ///< Simulated2D domain example factor } // \namespace inequality_constraints diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 8f9c9d09e..a62122705 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -20,7 +20,6 @@ using namespace boost; #include #include #include -#include #include #include @@ -33,14 +32,11 @@ typedef Cal3_S2Camera GeneralCamera; //typedef Cal3BundlerCamera GeneralCamera; typedef TypedSymbol CameraKey; typedef TypedSymbol PointKey; -typedef Values CameraConfig; -typedef Values PointConfig; -typedef TupleValues2 VisualValues; -typedef GeneralSFMFactor Projection; -typedef NonlinearEquality CameraConstraint; -typedef NonlinearEquality Point3Constraint; +typedef GeneralSFMFactor Projection; +typedef NonlinearEquality CameraConstraint; +typedef NonlinearEquality Point3Constraint; -class Graph: public NonlinearFactorGraph { +class Graph: public NonlinearFactorGraph { public: void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { push_back(boost::make_shared(z, model, i, j)); @@ -73,7 +69,7 @@ double getGaussian() return sqrt(-2.0f * (double)log(S) / S) * V1; } -typedef NonlinearOptimizer Optimizer; +typedef NonlinearOptimizer Optimizer; const SharedNoiseModel sigma1(noiseModel::Unit::Create(1)); @@ -101,12 +97,12 @@ TEST( GeneralSFMFactor, error ) { boost::shared_ptr 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 values(new VisualValues); + boost::shared_ptr 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 values(new VisualValues); + boost::shared_ptr 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 values(new VisualValues); + boost::shared_ptr 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 values(new VisualValues); + boost::shared_ptr 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 values(new VisualValues); + boost::shared_ptr 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]); diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 3f4e6df6f..89b5e5f34 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -20,7 +20,6 @@ using namespace boost; #include #include #include -#include #include #include @@ -32,15 +31,12 @@ using namespace gtsam; typedef Cal3BundlerCamera GeneralCamera; typedef TypedSymbol CameraKey; typedef TypedSymbol PointKey; -typedef Values CameraConfig; -typedef Values PointConfig; -typedef TupleValues2 VisualValues; -typedef GeneralSFMFactor Projection; -typedef NonlinearEquality CameraConstraint; -typedef NonlinearEquality Point3Constraint; +typedef GeneralSFMFactor Projection; +typedef NonlinearEquality CameraConstraint; +typedef NonlinearEquality Point3Constraint; /* ************************************************************************* */ -class Graph: public NonlinearFactorGraph { +class Graph: public NonlinearFactorGraph { public: void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { push_back(boost::make_shared(z, model, i, j)); @@ -73,7 +69,7 @@ double getGaussian() return sqrt(-2.0f * (double)log(S) / S) * V1; } -typedef NonlinearOptimizer Optimizer; +typedef NonlinearOptimizer Optimizer; const SharedNoiseModel sigma1(noiseModel::Unit::Create(1)); @@ -101,12 +97,12 @@ TEST( GeneralSFMFactor, error ) { boost::shared_ptr 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 values(new VisualValues); + boost::shared_ptr 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 values(new VisualValues); + boost::shared_ptr 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 values(new VisualValues); + boost::shared_ptr 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 values(new VisualValues); + boost::shared_ptr 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 values(new VisualValues); + boost::shared_ptr 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]); diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp index 2362e0ae5..3241a1e99 100644 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ b/gtsam/slam/tests/testPlanarSLAM.cpp @@ -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 > GNM = - *G.dynamicCastFactors > >(); + FactorGraph GNM = + *G.dynamicCastFactors >(); EXPECT(assert_equal(expected0, GNM[0]->unwhitenedError(c))); EXPECT(assert_equal(expected1, GNM[1]->unwhitenedError(c))); EXPECT(assert_equal(expected2, GNM[2]->unwhitenedError(c))); diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index c2c11b937..c17c20348 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -32,6 +32,7 @@ using namespace boost::assign; #include 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 > 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 initial(new Pose2Values()); - initial->insert(0, Pose2(0,0,0)); - initial->insert(1, Pose2(0,0,0)); + boost::shared_ptr 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(new Ordering); *ordering += "x0","x1"; - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer 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 fg(new Pose2Graph); @@ -188,10 +189,10 @@ TEST(Pose2Graph, optimizeThreePoses) { fg->addConstraint(2, 0, delta, covariance); // Create initial config - boost::shared_ptr 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 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(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 fg(new Pose2Graph); @@ -228,13 +229,13 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) { fg->addConstraint(5, 0, delta, covariance); // Create initial config - boost::shared_ptr 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 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(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, diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 949afea0f..574d05b6d 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -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 fg(new Pose3Graph); @@ -65,13 +66,13 @@ TEST(Pose3Graph, optimizeCircle) { fg->addConstraint(5,0, _0T1, covariance); // Create initial config - boost::shared_ptr 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 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(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 Partial; + typedef PartialPriorFactor 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 Partial; + typedef PartialPriorFactor 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)); } diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 09fd612d7..f5f41fd2e 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -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)); } diff --git a/gtsam/slam/tests/testSimulated3D.cpp b/gtsam/slam/tests/testSimulated3D.cpp index 30f41e2fd..5a90f2489 100644 --- a/gtsam/slam/tests/testSimulated3D.cpp +++ b/gtsam/slam/tests/testSimulated3D.cpp @@ -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)); diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 50a84d8b6..1fd23b57f 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -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 values(new visualSLAM::Values()); - values->insert(1, camera1); // add camera at z=6.25m looking towards origin + boost::shared_ptr 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 Optimizer; // optimization engine for this domain + typedef gtsam::NonlinearOptimizer Optimizer; // optimization engine for this domain double absoluteThreshold = 1e-9; double relativeThreshold = 1e-5; diff --git a/gtsam/slam/tests/testVSLAM.cpp b/gtsam/slam/tests/testVSLAM.cpp index c25b4260d..9281e51dc 100644 --- a/gtsam/slam/tests/testVSLAM.cpp +++ b/gtsam/slam/tests/testVSLAM.cpp @@ -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 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 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(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 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 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(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 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 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)); } diff --git a/tests/Makefile.am b/tests/Makefile.am index 2ed6139bb..433693bc3 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -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 diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 062a04ed6..d6bda8472 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -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 Graph; +typedef NonlinearFactorGraph Graph; typedef boost::shared_ptr shared_graph; typedef boost::shared_ptr shared_values; -typedef NonlinearOptimizer Optimizer; +typedef NonlinearOptimizer 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)); } diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 3a946858a..712c986ae 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -20,14 +20,12 @@ #include #include #include -#include #include using namespace gtsam; // Define Types for System Test typedef TypedSymbol TestKey; -typedef Values 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 ekf(x_initial, P_initial); + ExtendedKalmanFilter 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 factor1(x0, x1, difference, Q); + BetweenFactor factor1(x0, x1, difference, Q); Point2 predict1 = ekf.predict(factor1); EXPECT(assert_equal(expected1,predict1)); // Create the measurement factor - PriorFactor factor2(x1, z1, R); + PriorFactor factor2(x1, z1, R); Point2 update1 = ekf.update(factor2); EXPECT(assert_equal(expected1,update1)); // Run iteration 2 - BetweenFactor factor3(x1, x2, difference, Q); + BetweenFactor factor3(x1, x2, difference, Q); Point2 predict2 = ekf.predict(factor3); EXPECT(assert_equal(expected2,predict2)); - PriorFactor factor4(x2, z2, R); + PriorFactor factor4(x2, z2, R); Point2 update2 = ekf.update(factor4); EXPECT(assert_equal(expected2,update2)); // Run iteration 3 - BetweenFactor factor5(x2, x3, difference, Q); + BetweenFactor factor5(x2, x3, difference, Q); Point2 predict3 = ekf.predict(factor5); EXPECT(assert_equal(expected3,predict3)); - PriorFactor factor6(x3, z3, R); + PriorFactor 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 { +class NonlinearMotionModel : public NonlinearFactor2 { public: typedef TestKey::Value T; private: - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 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& f, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor2& f, double tol = 1e-9) const { const This *e = dynamic_cast (&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 linearize(const TestValues& c, const Ordering& ordering) const { + boost::shared_ptr 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 { +class NonlinearMeasurementModel : public NonlinearFactor1 { public: typedef TestKey::Value T; private: - typedef NonlinearFactor1 Base; + typedef NonlinearFactor1 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& f, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor1& f, double tol = 1e-9) const { const This *e = dynamic_cast (&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 linearize(const TestValues& c, const Ordering& ordering) const { + boost::shared_ptr 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 ekf(x_initial, P_initial); + ExtendedKalmanFilter ekf(x_initial, P_initial); // Enter Predict-Update Loop Point2 x_predict, x_update; diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index d28844cd9..7c39b8c60 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -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::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::Nodes nodesExpected( - 3, GaussianISAM2::sharedClique()); + GaussianISAM2<>::Nodes nodesExpected( + 3, GaussianISAM2<>::sharedClique()); // Expand initial state - GaussianISAM2::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::sharedClique clique( - GaussianISAM2::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& 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 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::sharedClique sharedClique; + typedef GaussianISAM2<>::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph 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 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::sharedClique sharedClique; + typedef GaussianISAM2<>::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph 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 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 > isam2 - = boost::shared_ptr >(new GaussianISAM2()); + boost::shared_ptr > isam2 + = boost::shared_ptr >(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 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 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::sharedClique sharedClique; + typedef GaussianISAM2<>::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph 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 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::sharedClique sharedClique; + typedef GaussianISAM2<>::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph jfg; diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index c7418a673..fc9662519 100644 --- a/tests/testGaussianJunctionTree.cpp +++ b/tests/testGaussianJunctionTree.cpp @@ -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"; diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index e9698a753..f6d8629bf 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -86,22 +86,22 @@ TEST( Graph, composePoses ) graph.addConstraint(2,3, p23, cov); graph.addConstraint(4,3, p43, cov); - PredecessorMap tree; - tree.insert(1,2); - tree.insert(2,2); - tree.insert(3,2); - tree.insert(4,3); + PredecessorMap 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 actual = composePoses (graph, tree, rootPose); + boost::shared_ptr actual = composePoses (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)); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 501bb9db8..b917185e9 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -32,20 +32,19 @@ namespace eq2D = gtsam::simulated2D::equality_constraints; static const double tol = 1e-5; typedef TypedSymbol PoseKey; -typedef Values PoseValues; -typedef PriorFactor PosePrior; -typedef NonlinearEquality PoseNLE; +typedef PriorFactor PosePrior; +typedef NonlinearEquality PoseNLE; typedef boost::shared_ptr shared_poseNLE; -typedef NonlinearFactorGraph PoseGraph; -typedef NonlinearOptimizer PoseOptimizer; +typedef NonlinearFactorGraph PoseGraph; +typedef NonlinearOptimizer 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 init(new PoseValues()); + boost::shared_ptr 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 init(new PoseValues()); + boost::shared_ptr 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 Graph; +typedef NonlinearFactorGraph Graph; typedef boost::shared_ptr shared_graph; -typedef boost::shared_ptr shared_values; -typedef NonlinearOptimizer Optimizer; +typedef boost::shared_ptr shared_values; +typedef NonlinearOptimizer 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 shK(new Cal3_S2(K)); // typedefs for visual SLAM example -typedef visualSLAM::Values VValues; -typedef boost::shared_ptr shared_vconfig; +typedef boost::shared_ptr shared_vconfig; typedef visualSLAM::Graph VGraph; -typedef NonlinearOptimizer VOptimizer; +typedef NonlinearOptimizer VOptimizer; // factors for visual slam -typedef NonlinearEquality2 Point3Equality; +typedef NonlinearEquality2 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); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 0d5402607..c9fa05c45 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -40,7 +40,7 @@ using namespace std; using namespace gtsam; using namespace example; -typedef boost::shared_ptr > shared_nlf; +typedef boost::shared_ptr 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 >(factor)->unwhitenedError(cfg); + Vector actual_e = boost::dynamic_pointer_cast(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 TestKey; -typedef gtsam::Values TestValues; /* ************************************************************************* */ -class TestFactor4 : public NonlinearFactor4 { +class TestFactor4 : public NonlinearFactor4 { public: - typedef NonlinearFactor4 Base; + typedef NonlinearFactor4 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 { +class TestFactor5 : public NonlinearFactor5 { public: - typedef NonlinearFactor5 Base; + typedef NonlinearFactor5 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 { +class TestFactor6 : public NonlinearFactor6 { public: - typedef NonlinearFactor6 Base; + typedef NonlinearFactor6 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); diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index b90e98625..66fd410b6 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -12,7 +12,7 @@ using namespace gtsam; using namespace planarSLAM; -typedef NonlinearISAM 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); diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index ccc1748dc..57366500e 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -44,7 +44,7 @@ using namespace gtsam; const double tol = 1e-5; -typedef NonlinearOptimizer Optimizer; +typedef NonlinearOptimizer 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( + DynamicValues actualMFQR = optimize( fg, c0, *NonlinearOptimizationParameters().newFactorization(true), MULTIFRONTAL, LM); DOUBLES_EQUAL(0,fg.error(actualMFQR),tol); - example::Values actualMFLDL = optimize( + DynamicValues actualMFLDL = optimize( 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 Optimizer; + typedef NonlinearOptimizer Optimizer; - boost::shared_ptr config(new Pose2Values); - config->insert(1, Pose2(0.,0.,0.)); - config->insert(2, Pose2(1.5,0.,0.)); + boost::shared_ptr 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 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(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)); } diff --git a/tests/testPose2SLAMwSPCG.cpp b/tests/testPose2SLAMwSPCG.cpp index 1b3d9f455..64585ff75 100644 --- a/tests/testPose2SLAMwSPCG.cpp +++ b/tests/testPose2SLAMwSPCG.cpp @@ -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)); } diff --git a/tests/testRot3Optimization.cpp b/tests/testRot3Optimization.cpp index a35165273..1e57a0797 100644 --- a/tests/testRot3Optimization.cpp +++ b/tests/testRot3Optimization.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include #include @@ -31,27 +30,26 @@ using namespace gtsam; typedef TypedSymbol Key; -typedef Values Rot3Values; -typedef PriorFactor Prior; -typedef BetweenFactor Between; -typedef NonlinearFactorGraph Graph; +typedef PriorFactor Prior; +typedef BetweenFactor 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)); }