From 15cfa7e70239045f3fafa7078d47b0b990c431d1 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 30 Jul 2012 15:38:58 +0000 Subject: [PATCH] Remove namespace files --- gtsam/slam/planarSLAM.cpp | 63 --- gtsam/slam/planarSLAM.h | 122 ------ gtsam/slam/pose2SLAM.cpp | 66 --- gtsam/slam/pose2SLAM.h | 107 ----- gtsam/slam/pose3SLAM.cpp | 73 ---- gtsam/slam/pose3SLAM.h | 120 ------ gtsam/slam/sparseBA.cpp | 60 --- gtsam/slam/sparseBA.h | 143 ------- gtsam/slam/tests/testPlanarSLAM.cpp | 218 ---------- gtsam/slam/tests/testPose2SLAM.cpp | 608 ---------------------------- gtsam/slam/tests/testPose3SLAM.cpp | 211 ---------- gtsam/slam/tests/testSparseBA.cpp | 193 --------- gtsam/slam/tests/testVisualSLAM.cpp | 344 ---------------- gtsam/slam/visualSLAM.cpp | 116 ------ gtsam/slam/visualSLAM.h | 186 --------- 15 files changed, 2630 deletions(-) delete mode 100644 gtsam/slam/planarSLAM.cpp delete mode 100644 gtsam/slam/planarSLAM.h delete mode 100644 gtsam/slam/pose2SLAM.cpp delete mode 100644 gtsam/slam/pose2SLAM.h delete mode 100644 gtsam/slam/pose3SLAM.cpp delete mode 100644 gtsam/slam/pose3SLAM.h delete mode 100644 gtsam/slam/sparseBA.cpp delete mode 100644 gtsam/slam/sparseBA.h delete mode 100644 gtsam/slam/tests/testPlanarSLAM.cpp delete mode 100644 gtsam/slam/tests/testPose2SLAM.cpp delete mode 100644 gtsam/slam/tests/testPose3SLAM.cpp delete mode 100644 gtsam/slam/tests/testSparseBA.cpp delete mode 100644 gtsam/slam/tests/testVisualSLAM.cpp delete mode 100644 gtsam/slam/visualSLAM.cpp delete mode 100644 gtsam/slam/visualSLAM.h diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp deleted file mode 100644 index 20c149b66..000000000 --- a/gtsam/slam/planarSLAM.cpp +++ /dev/null @@ -1,63 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file planarSLAM.cpp - * @brief: bearing/range measurements in 2D plane - * @author Frank Dellaert - **/ - -#include -#include - -// Use planarSLAM namespace for specific SLAM instance -namespace planarSLAM { - - /* ************************************************************************* */ - Matrix Values::points() const { - size_t j=0; - ConstFiltered points = filter(); - Matrix result(points.size(),2); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, points) - result.row(j++) = keyValue.value.vector(); - return result; - } - - /* ************************************************************************* */ - void Graph::addPointConstraint(Key pointKey, const Point2& p) { - push_back(boost::make_shared >(pointKey, p)); - } - - /* ************************************************************************* */ - void Graph::addPointPrior(Key pointKey, const Point2& p, const SharedNoiseModel& model) { - push_back(boost::make_shared >(pointKey, p, model)); - } - - /* ************************************************************************* */ - void Graph::addBearing(Key i, Key j, const Rot2& z, const SharedNoiseModel& model) { - push_back(boost::make_shared >(i, j, z, model)); - } - - /* ************************************************************************* */ - void Graph::addRange(Key i, Key j, double z, const SharedNoiseModel& model) { - push_back(boost::make_shared >(i, j, z, model)); - } - - /* ************************************************************************* */ - void Graph::addBearingRange(Key i, Key j, const Rot2& z1, - double z2, const SharedNoiseModel& model) { - push_back(boost::make_shared >(i, j, z1, z2, model)); - } - - /* ************************************************************************* */ - -} // planarSLAM - diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h deleted file mode 100644 index 967d91581..000000000 --- a/gtsam/slam/planarSLAM.h +++ /dev/null @@ -1,122 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file planarSLAM.h - * @brief bearing/range measurements in 2D plane - * @author Frank Dellaert - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -// Use planarSLAM namespace for specific SLAM instance -namespace planarSLAM { - - using namespace gtsam; - - /* - * Values class, inherited from pose2SLAM::Values, mainly used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Values: public pose2SLAM::Values { - - typedef boost::shared_ptr shared_ptr; - typedef gtsam::Values::ConstFiltered PoseFiltered; - typedef gtsam::Values::ConstFiltered PointFiltered; - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const gtsam::Values& values) : - pose2SLAM::Values(values) { - } - - /// Constructor from filtered values view of poses - Values(const PoseFiltered& view) : pose2SLAM::Values(view) {} - - /// Constructor from filtered values view of points - Values(const PointFiltered& view) : pose2SLAM::Values(view) {} - - PoseFiltered allPoses() const { return this->filter(); } ///< pose view - size_t nrPoses() const { return allPoses().size(); } ///< get number of poses - KeyList poseKeys() const { return allPoses().keys(); } ///< get keys to poses only - - PointFiltered allPoints() const { return this->filter(); } ///< point view - size_t nrPoints() const { return allPoints().size(); } ///< get number of points - KeyList pointKeys() const { return allPoints().keys(); } ///< get keys to points only - - /// insert a point - void insertPoint(Key j, const Point2& point) { insert(j, point); } - - /// update a point - void updatePoint(Key j, const Point2& point) { update(j, point); } - - /// get a point - Point2 point(Key j) const { return at(j); } - - /// get all [x,y] coordinates in a 2*n matrix - Matrix points() const; -}; - - /** - * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Graph: public pose2SLAM::Graph { - - /// Default constructor - Graph(){} - - /// Copy constructor given any other NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph): - pose2SLAM::Graph(graph) {} - - /// Creates a hard constraint on a point - void addPointConstraint(Key j, const Point2& p); - - /// Adds a prior with mean p and given noise model on point j - void addPointPrior(Key j, const Point2& p, const SharedNoiseModel& model); - - /// Creates a bearing measurement from pose i to point j - void addBearing(Key i, Key j, const Rot2& bearing, const SharedNoiseModel& model); - - /// Creates a range measurement from pose i to point j - void addRange(Key i, Key k, double range, const SharedNoiseModel& model); - - /// Creates a range/bearing measurement from pose i to point j - void addBearingRange(Key i, Key j, const Rot2& bearing, double range, const SharedNoiseModel& model); - }; - -} // planarSLAM - - -/** - * Backwards compatibility and wrap use only, avoid using - */ -namespace planarSLAM { - typedef gtsam::NonlinearEquality Constraint; ///< \deprecated typedef for backwards compatibility - typedef gtsam::PriorFactor Prior; ///< \deprecated typedef for backwards compatibility - typedef gtsam::BetweenFactor Odometry; ///< \deprecated typedef for backwards compatibility - typedef gtsam::BearingFactor Bearing; ///< \deprecated typedef for backwards compatibility - typedef gtsam::RangeFactor Range; ///< \deprecated typedef for backwards compatibility - typedef gtsam::BearingRangeFactor BearingRange; ///< \deprecated typedef for backwards compatibility -} - - diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp deleted file mode 100644 index cc549d84a..000000000 --- a/gtsam/slam/pose2SLAM.cpp +++ /dev/null @@ -1,66 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file pose2SLAM.cpp - * @brief: Odometry measurements in 2D plane - * @author Frank Dellaert - **/ - -#include - -// Use pose2SLAM namespace for specific SLAM instance - -namespace pose2SLAM { - - /* ************************************************************************* */ - Values Values::Circle(size_t n, double R) { - Values x; - double theta = 0, dtheta = 2 * M_PI / n; - for (size_t i = 0; i < n; i++, theta += dtheta) - x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta)); - return x; - } - - /* ************************************************************************* */ - Matrix Values::poses() const { - size_t j=0; - ConstFiltered poses = filter(); - Matrix result(poses.size(),3); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) { - const Pose2& r = keyValue.value; - result.row(j++) = Matrix_(1,3, r.x(), r.y(), r.theta()); - } - return result; - } - - /* ************************************************************************* */ - void Graph::addPoseConstraint(Key i, const Pose2& p) { - sharedFactor factor(new NonlinearEquality(i, p)); - push_back(factor); - } - - /* ************************************************************************* */ - void Graph::addPosePrior(Key i, const Pose2& p, const SharedNoiseModel& model) { - sharedFactor factor(new PriorFactor(i, p, model)); - push_back(factor); - } - - /* ************************************************************************* */ - void Graph::addRelativePose(Key i1, Key i2, const Pose2& z, - const SharedNoiseModel& model) { - sharedFactor factor(new BetweenFactor(i1, i2, z, model)); - push_back(factor); - } - - /* ************************************************************************* */ - -} // pose2SLAM diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h deleted file mode 100644 index 018090c6a..000000000 --- a/gtsam/slam/pose2SLAM.h +++ /dev/null @@ -1,107 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file pose2SLAM.h - * @brief: 2D Pose SLAM - * @author Frank Dellaert - **/ - -#pragma once - -#include -#include -#include -#include -#include - -// Use pose2SLAM namespace for specific SLAM instance -namespace pose2SLAM { - - using namespace gtsam; - - /* - * Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Values: public gtsam::Values { - - typedef boost::shared_ptr shared_ptr; - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const gtsam::Values& values) : - gtsam::Values(values) { - } - - /** - * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) - * @param n number of poses - * @param R radius of circle - * @param c character to use for keys - * @return circle of n 2D poses - */ - static Values Circle(size_t n, double R); - - /// insert a pose - void insertPose(Key i, const Pose2& pose) { insert(i, pose); } - - /// update a pose - void updatePose(Key i, const Pose2& pose) { update(i, pose); } - - /// get a pose - Pose2 pose(Key i) const { return at(i); } - - /// get all [x,y,theta] coordinates in a 3*m matrix - Matrix poses() const; - }; - - /** - * List of typedefs for factors - */ - - /** - * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Graph: public EasyFactorGraph { - - typedef boost::shared_ptr shared_ptr; - - /// Default constructor - Graph(){} - - /// Copy constructor given any other NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph): - EasyFactorGraph(graph) {} - - /// Creates a hard constraint for key i with the given Pose2 p. - void addPoseConstraint(Key i, const Pose2& p); - - /// Adds a Pose2 prior with mean p and given noise model on pose i - void addPosePrior(Key i, const Pose2& p, const SharedNoiseModel& model); - - /// Creates an relative pose factor between poses with keys i1 and i2 - void addRelativePose(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model); - }; - -} // pose2SLAM - -/** - * Backwards compatibility and wrap use only, avoid using - */ -namespace pose2SLAM { - typedef gtsam::NonlinearEquality HardConstraint; ///< \deprecated typedef for backwards compatibility - typedef gtsam::PriorFactor Prior; ///< \deprecated typedef for backwards compatibility - typedef gtsam::BetweenFactor Odometry; ///< \deprecated typedef for backwards compatibility -} diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp deleted file mode 100644 index 07a89e419..000000000 --- a/gtsam/slam/pose3SLAM.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file pose3SLAM.cpp - * @brief: bearing/range measurements in 2D plane - * @author Frank Dellaert - **/ - -#include -#include - -// Use pose3SLAM namespace for specific SLAM instance -namespace pose3SLAM { - - /* ************************************************************************* */ - Values Values::Circle(size_t n, double radius) { - Values x; - double theta = 0, dtheta = 2 * M_PI / n; - // We use aerospace/navlab convention, X forward, Y right, Z down - // First pose will be at (R,0,0) - // ^y ^ X - // | | - // z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) - // Vehicle at p0 is looking towards y axis (X-axis points towards world y) - Rot3 gR0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); - for (size_t i = 0; i < n; i++, theta += dtheta) { - Point3 gti(radius*cos(theta), radius*sin(theta), 0); - Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down ! - Pose3 gTi(gR0 * _0Ri, gti); - x.insert(i, gTi); - } - return x; - } - - /* ************************************************************************* */ - Matrix Values::translations() const { - size_t j=0; - ConstFiltered poses = filter(); - Matrix result(poses.size(),3); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) - result.row(j++) = keyValue.value.translation().vector(); - return result; - } - - /* ************************************************************************* */ - void Graph::addPoseConstraint(Key i, const Pose3& p) { - sharedFactor factor(new NonlinearEquality(i, p)); - push_back(factor); - } - - /* ************************************************************************* */ - void Graph::addPosePrior(Key i, const Pose3& p, const SharedNoiseModel& model) { - sharedFactor factor(new PriorFactor(i, p, model)); - push_back(factor); - } - - /* ************************************************************************* */ - void Graph::addRelativePose(Key i1, Key i2, const Pose3& z, const SharedNoiseModel& model) { - push_back(boost::make_shared >(i1, i2, z, model)); - } - - /* ************************************************************************* */ - -} // pose3SLAM diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h deleted file mode 100644 index 20ea8b0d5..000000000 --- a/gtsam/slam/pose3SLAM.h +++ /dev/null @@ -1,120 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file pose3SLAM.h - * @brief: 3D Pose SLAM - * @author Frank Dellaert - **/ - -#pragma once - -#include -#include -#include -#include -#include - -/// Use pose3SLAM namespace for specific SLAM instance -namespace pose3SLAM { - - using namespace gtsam; - - /* - * Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Values: public gtsam::Values { - - typedef boost::shared_ptr shared_ptr; - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const gtsam::Values& values) : - gtsam::Values(values) { - } - - /** - * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) - * @param n number of poses - * @param R radius of circle - * @return circle of n 3D poses - */ - static Values Circle(size_t n, double R); - - /// insert a pose - void insertPose(Key i, const Pose3& pose) { insert(i, pose); } - - /// update a pose - void updatePose(Key i, const Pose3& pose) { update(i, pose); } - - /// get a pose - Pose3 pose(Key i) const { return at(i); } - - Matrix translations() const; ///< get all pose translations coordinates in a matrix - }; - - /** - * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Graph: public EasyFactorGraph { - - /// Default constructor - Graph(){} - - /// Copy constructor given any other NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph): - EasyFactorGraph(graph) {} - - /** - * Add a prior on a pose - * @param key variable key of the camera pose - * @param p around which soft prior is defined - * @param model uncertainty model of this prior - */ - void addPosePrior(Key poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)); - - /** - * Add a constraint on a pose (for now, *must* be satisfied in any Values) - * @param key variable key of the camera pose - * @param p to which pose to constrain it to - */ - void addPoseConstraint(Key poseKey, const Pose3& p = Pose3()); - - /** - * Add a relative pose measurement between two poses - * @param x1 variable key of the first camera pose - * @param x2 variable key of the second camera pose - * @param relative pose measurement from x1 to x2 (x1.between(x2)) - * @param model uncertainty model of this measurement - */ - void addRelativePose(Key x1, Key x2, const Pose3& z, const SharedNoiseModel& model); - }; - -} // pose3SLAM - -/** - * Backwards compatibility and wrap use only, avoid using - */ -namespace pose3SLAM { - typedef gtsam::PriorFactor Prior; ///< \deprecated typedef for backwards compatibility - typedef gtsam::BetweenFactor Constraint; ///< \deprecated typedef for backwards compatibility - typedef gtsam::NonlinearEquality HardConstraint; ///< \deprecated typedef for backwards compatibility -} -namespace gtsam { - typedef pose3SLAM::Prior Pose3Prior; ///< \deprecated typedef for backwards compatibility - typedef pose3SLAM::Constraint Pose3Factor; ///< \deprecated typedef for backwards compatibility - typedef pose3SLAM::Graph Pose3Graph; ///< \deprecated typedef for backwards compatibility -} - diff --git a/gtsam/slam/sparseBA.cpp b/gtsam/slam/sparseBA.cpp deleted file mode 100644 index 0ff4e1c64..000000000 --- a/gtsam/slam/sparseBA.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/** - * @file sparseBA.cpp - * @brief - * @date Jul 6, 2012 - * @author Yong-Dian Jian - */ - -#include - -namespace sparseBA { - -/* ************************************************************************* */ -void Graph::addSimpleCameraConstraint(Key cameraKey, const SimpleCamera &camera) { - addCameraConstraint(cameraKey, camera); -} - -/* ************************************************************************* */ -void Graph::addSimpleCameraPrior(Key cameraKey, const SimpleCamera &camera, SharedNoiseModel &model) { - addCameraPrior(cameraKey, camera, model); -} - -/* ************************************************************************* */ -void Graph::addSimpleCameraMeasurement(const Point2 &z, SharedNoiseModel& model, Index cameraKey, Index pointKey) { - addMeasurement(z, model, cameraKey, pointKey); -} - -/* ************************************************************************* */ -Matrix Graph::reprojectionErrors(const Values& values) const { - - // TODO: support the other calibration objects. Now it only works for Cal3_S2. - - typedef GeneralSFMFactor SFMFactor; - typedef GeneralSFMFactor2 SFMFactor2; - - // first count - size_t K = 0, k=0; - BOOST_FOREACH(const sharedFactor& f, *this) - if (boost::dynamic_pointer_cast(f)) ++K; - else if (boost::dynamic_pointer_cast(f)) ++K; - - // now fill - Matrix errors(2,K); - BOOST_FOREACH(const sharedFactor& f, *this) { - boost::shared_ptr p = boost::dynamic_pointer_cast(f); - if (p) { - errors.col(k++) = p->unwhitenedError(values); - continue; - } - - boost::shared_ptr p2 = boost::dynamic_pointer_cast(f); - if (p2) { - errors.col(k++) = p2->unwhitenedError(values); - } - } - return errors; -} -/* ************************************************************************* */ -} - - diff --git a/gtsam/slam/sparseBA.h b/gtsam/slam/sparseBA.h deleted file mode 100644 index 07aaa5817..000000000 --- a/gtsam/slam/sparseBA.h +++ /dev/null @@ -1,143 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file sba.h - * @brief a suite for sparse bundle adjustment - * @date Jul 5, 2012 - * @author ydjian - */ - -#pragma once - -#include -#include -#include - -namespace sparseBA { - - using namespace gtsam; - - /// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper - struct Values: public visualSLAM::Values { - - typedef boost::shared_ptr shared_ptr; - typedef gtsam::Values::ConstFiltered SimpleCameraFiltered; - typedef gtsam::Values::ConstFiltered Cal3_S2Filtered; - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const gtsam::Values& values) : visualSLAM::Values(values) {} - - /// Constructor from filtered values view of poses - Values(const SimpleCameraFiltered& view) : visualSLAM::Values(view) {} - - /// Constructor from filtered values view of points - Values(const PointFiltered& view) : visualSLAM::Values(view) {} - - SimpleCameraFiltered allSimpleCameras() const { return this->filter(); } ///< camera view - size_t nrSimpleCameras() const { return allSimpleCameras().size(); } ///< get number of poses - KeyList simpleCameraKeys() const { return allSimpleCameras().keys(); } ///< get keys to poses only - - /// insert a camera - void insertSimpleCamera(Key j, const SimpleCamera& camera) { insert(j, camera); } - - /// update a camera - void updateSimpleCamera(Key j, const SimpleCamera& camera) { update(j, camera); } - - /// get a camera - SimpleCamera simpleCamera(Key j) const { return at(j); } - }; - - /** - * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Graph: public visualSLAM::Graph { - - /// Default constructor - Graph(){} - - /// Copy constructor given any other NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph): visualSLAM::Graph(graph) {} - - /// check if two graphs are equal - bool equals(const Graph& p, double tol = 1e-9) const { - return NonlinearFactorGraph::equals(p, tol); - } - - /** - * Add a prior on a pose - * @param key variable key of the camera - * @param p around which soft prior is defined - * @param model uncertainty model of this prior - */ - template - void addCameraPrior(Key cameraKey, const Camera &camera, SharedNoiseModel &model = noiseModel::Unit::Create(Camera::Dim())) { - sharedFactor factor(new PriorFactor(cameraKey, camera, model)); - push_back(factor); - } - - /** - * Add a constraint on a camera - * @param key variable key of the camera - * @param p to which camera to constrain it to - */ - template - void addCameraConstraint(Key cameraKey, const Camera &camera) { - sharedFactor factor(new NonlinearEquality(cameraKey, camera)); - push_back(factor); - } - - /** - * Add a 2d projection measurement - * @param z the measurement - * @param model the noise model for the measurement - * @param cameraKey variable key for the pose+calibration - * @param pointKey variable key for the point - */ - template - void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index cameraKey, const Index pointKey) { - typedef GeneralSFMFactor SFMFactor; - boost::shared_ptr factor(new SFMFactor(z, model, cameraKey, pointKey)); - push_back(factor); - } - - /** - * Add a 2d projection measurement, but supports separated (or shared) pose and calibration object - * @param z the measurement - * @param model the noise model for the measurement - * @param poseKey variable key for the pose - * @param pointKey variable key for the point - * @param calibKey variable key for the calibration - */ - template - void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index posekey, const Index pointkey, const Index calibkey) { - typedef GeneralSFMFactor2 SFMFactor; - boost::shared_ptr factor(new SFMFactor(z, model, posekey, pointkey, calibkey)); - push_back(factor); - } - - /// Return a 2*K Matrix of reprojection errors - Matrix reprojectionErrors(const Values& values) const; - - /** - * Matlab-specific wrappers - */ - - void addSimpleCameraPrior(Key cameraKey, const SimpleCamera &camera, SharedNoiseModel &model); - void addSimpleCameraConstraint(Key cameraKey, const SimpleCamera &camera) ; - void addSimpleCameraMeasurement(const Point2 &z, SharedNoiseModel& model, Index cameraKey, Index pointKey); - }; - -} diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp deleted file mode 100644 index 5652b71b4..000000000 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ /dev/null @@ -1,218 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testPlanarSLAM.cpp - * @author Frank Dellaert - **/ - -#include -#include -#include -#include - -#include -#include - -using namespace std; -using namespace gtsam; - -// some shared test values -static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); -static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); -static Symbol i2('x',2), i3('x',3), j3('l',3); - -SharedNoiseModel - sigma(noiseModel::Isotropic::Sigma(1,0.1)), - sigma2(noiseModel::Isotropic::Sigma(2,0.1)), - I3(noiseModel::Unit::Create(3)); - -/* ************************************************************************* */ -TEST( planarSLAM, PriorFactor_equals ) -{ - PriorFactor factor1(i2, x1, I3), factor2(i2, x2, I3); - EXPECT(assert_equal(factor1, factor1, 1e-5)); - EXPECT(assert_equal(factor2, factor2, 1e-5)); - EXPECT(assert_inequal(factor1, factor2, 1e-5)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, BearingFactor ) -{ - // Create factor - Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 - BearingFactor factor(i2, j3, z, sigma); - - // create config - planarSLAM::Values c; - c.insert(i2, x2); - c.insert(j3, l3); - - // Check error - Vector actual = factor.unwhitenedError(c); - EXPECT(assert_equal(Vector_(1,-0.1),actual)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, BearingFactor_equals ) -{ - BearingFactor - factor1(i2, j3, Rot2::fromAngle(0.1), sigma), - factor2(i2, j3, Rot2::fromAngle(2.3), sigma); - EXPECT(assert_equal(factor1, factor1, 1e-5)); - EXPECT(assert_equal(factor2, factor2, 1e-5)); - EXPECT(assert_inequal(factor1, factor2, 1e-5)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, RangeFactor ) -{ - // Create factor - double z(sqrt(2.0) - 0.22); // h(x) - z = 0.22 - RangeFactor factor(i2, j3, z, sigma); - - // create config - planarSLAM::Values c; - c.insert(i2, x2); - c.insert(j3, l3); - - // Check error - Vector actual = factor.unwhitenedError(c); - EXPECT(assert_equal(Vector_(1,0.22),actual)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, RangeFactor_equals ) -{ - RangeFactor factor1(i2, j3, 1.2, sigma), factor2(i2, j3, 7.2, sigma); - EXPECT(assert_equal(factor1, factor1, 1e-5)); - EXPECT(assert_equal(factor2, factor2, 1e-5)); - EXPECT(assert_inequal(factor1, factor2, 1e-5)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, BearingRangeFactor ) -{ - // Create factor - Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 - double b(sqrt(2.0) - 0.22); // h(x) - z = 0.22 - BearingRangeFactor factor(i2, j3, r, b, sigma2); - - // create config - planarSLAM::Values c; - c.insert(i2, x2); - c.insert(j3, l3); - - // Check error - Vector actual = factor.unwhitenedError(c); - EXPECT(assert_equal(Vector_(2,-0.1, 0.22),actual)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, BearingRangeFactor_equals ) -{ - BearingRangeFactor - factor1(i2, j3, Rot2::fromAngle(0.1), 7.3, sigma2), - factor2(i2, j3, Rot2::fromAngle(3), 2.0, sigma2); - EXPECT(assert_equal(factor1, factor1, 1e-5)); - EXPECT(assert_equal(factor2, factor2, 1e-5)); - EXPECT(assert_inequal(factor1, factor2, 1e-5)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, BearingRangeFactor_poses ) -{ - typedef BearingRangeFactor PoseBearingRange; - PoseBearingRange actual(2, 3, Rot2::fromDegrees(60.0), 12.3, sigma2); -} - -/* ************************************************************************* */ -TEST( planarSLAM, PoseConstraint_equals ) -{ - NonlinearEquality factor1(i2, x2), factor2(i2, x3); - EXPECT(assert_equal(factor1, factor1, 1e-5)); - EXPECT(assert_equal(factor2, factor2, 1e-5)); - EXPECT(assert_inequal(factor1, factor2, 1e-5)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, constructor ) -{ - // create config - planarSLAM::Values c; - c.insert(i2, x2); - c.insert(i3, x3); - c.insert(j3, l3); - - // create graph - planarSLAM::Graph G; - - // Add pose constraint - G.addPoseConstraint(i2, x2); // make it feasible :-) - - // Add odometry - G.addRelativePose(i2, i3, Pose2(0, 0, M_PI_4), I3); - - // Create bearing factor - Rot2 z1 = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 - G.addBearing(i2, j3, z1, sigma); - - // Create range factor - double z2(sqrt(2.0) - 0.22); // h(x) - z = 0.22 - G.addRange(i2, j3, z2, sigma); - - Vector expected0 = Vector_(3, 0.0, 0.0, 0.0); - Vector expected1 = Vector_(3, 0.0, 0.0, 0.0); - Vector expected2 = Vector_(1, -0.1); - Vector expected3 = Vector_(1, 0.22); - // Get NoiseModelFactors - EXPECT(assert_equal(expected0, boost::dynamic_pointer_cast(G[0])->unwhitenedError(c))); - EXPECT(assert_equal(expected1, boost::dynamic_pointer_cast(G[1])->unwhitenedError(c))); - EXPECT(assert_equal(expected2, boost::dynamic_pointer_cast(G[2])->unwhitenedError(c))); - EXPECT(assert_equal(expected3, boost::dynamic_pointer_cast(G[3])->unwhitenedError(c))); -} - -/* ************************************************************************* */ -TEST( planarSLAM, keys_and_view ) -{ - // create config - planarSLAM::Values c; - c.insert(i2, x2); - c.insert(i3, x3); - c.insert(j3, l3); - LONGS_EQUAL(2,c.nrPoses()); - LONGS_EQUAL(1,c.nrPoints()); - { - FastList expected, actual; - expected += j3, i2, i3; - actual = c.keys(); - CHECK(expected == actual); - } - { - FastList expected, actual; - expected += i2, i3; - actual = c.poseKeys(); - CHECK(expected == actual); - } - { - FastList expected, actual; - expected += j3; - actual = c.pointKeys(); - CHECK(expected == actual); - } -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp deleted file mode 100644 index 766d06ded..000000000 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ /dev/null @@ -1,608 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testPose2SLAM.cpp - * @author Frank Dellaert, Viorela Ila - **/ - -#include -#include -#include -#include -#include -#include -using namespace gtsam; - -#include - -#include -#include -using namespace boost; -using namespace boost::assign; - -#include -using namespace std; - -typedef BetweenFactor Pose2Factor; - -// common measurement covariance -static double sx=0.5, sy=0.5,st=0.1; -static Matrix cov(Matrix_(3, 3, - sx*sx, 0.0, 0.0, - 0.0, sy*sy, 0.0, - 0.0, 0.0, st*st - )); -static noiseModel::Gaussian::shared_ptr covariance(noiseModel::Gaussian::Covariance(cov)); - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2SLAM, XYT ) -{ - pose2SLAM::Values values; - values.insertPose(1,Pose2(1,2,3)); - values.insertPose(2,Pose2(4,5,6)); - Matrix expected = Matrix_(2,3, 1.0,2.0,3.0, 4.0,5.0,6.0-2*M_PI); - EXPECT(assert_equal(expected,values.poses())); -} - -/* ************************************************************************* */ -// Test constraint, small displacement -Vector f1(const Pose2& pose1, const Pose2& pose2) { - Pose2 z(2.1130913087, 0.468461064817, 0.436332312999); - Pose2Factor constraint(1, 2, z, covariance); - return constraint.evaluateError(pose1, pose2); -} - -TEST_UNSAFE( Pose2SLAM, constraint1 ) -{ - // create a factor between unknown poses p1 and p2 - Pose2 pose1, pose2(2.1130913087, 0.468461064817, 0.436332312999); - Pose2Factor constraint(1, 2, pose2, covariance); - Matrix H1, H2; - Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); - - Matrix numericalH1 = numericalDerivative21(&f1 , pose1, pose2); - EXPECT(assert_equal(numericalH1,H1,5e-3)); - - Matrix numericalH2 = numericalDerivative22(&f1 , pose1, pose2); - EXPECT(assert_equal(numericalH2,H2)); -} - -/* ************************************************************************* */ -// Test constraint, large displacement -Vector f2(const Pose2& pose1, const Pose2& pose2) { - Pose2 z(2,2,M_PI_2); - Pose2Factor constraint(1, 2, z, covariance); - return constraint.evaluateError(pose1, pose2); -} - -TEST_UNSAFE( Pose2SLAM, constraint2 ) -{ - // create a factor between unknown poses p1 and p2 - Pose2 pose1, pose2(2,2,M_PI_2); - Pose2Factor constraint(1, 2, pose2, covariance); - Matrix H1, H2; - Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); - - Matrix numericalH1 = numericalDerivative21(&f2 , pose1, pose2); - EXPECT(assert_equal(numericalH1,H1,5e-3)); - - Matrix numericalH2 = numericalDerivative22(&f2 , pose1, pose2); - EXPECT(assert_equal(numericalH2,H2)); -} - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2SLAM, constructor ) -{ - // create a factor between unknown poses p1 and p2 - Pose2 measured(2,2,M_PI_2); - pose2SLAM::Graph graph; - graph.addRelativePose(1,2,measured, covariance); - // get the size of the graph - size_t actual = graph.size(); - // verify - size_t expected = 1; - CHECK(actual == expected); -} - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2SLAM, linearization ) -{ - // create a factor between unknown poses p1 and p2 - Pose2 measured(2,2,M_PI_2); - Pose2Factor constraint(1, 2, measured, covariance); - pose2SLAM::Graph graph; - graph.addRelativePose(1,2,measured, covariance); - - // Choose a linearization point - Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) - Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2) - pose2SLAM::Values config; - config.insert(1,p1); - config.insert(2,p2); - // Linearize - Ordering ordering(*config.orderingArbitrary()); - boost::shared_ptr > lfg_linearized = graph.linearize(config, ordering); - //lfg_linearized->print("lfg_actual"); - - // the expected linear factor - FactorGraph lfg_expected; - Matrix A1 = Matrix_(3,3, - 0.0,-2.0, -4.2, - 2.0, 0.0, -4.2, - 0.0, 0.0,-10.0); - - Matrix A2 = Matrix_(3,3, - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 10.0); - - Vector b = Vector_(3,-0.1/sx,0.1/sy,0.0); - SharedDiagonal probModel1 = noiseModel::Unit::Create(3); - lfg_expected.push_back(JacobianFactor::shared_ptr(new JacobianFactor(ordering[1], A1, ordering[2], A2, b, probModel1))); - - CHECK(assert_equal(lfg_expected, *lfg_linearized)); -} - -/* ************************************************************************* */ -TEST_UNSAFE(Pose2SLAM, optimize) { - - // create a Pose graph with one equality constraint and one measurement - pose2SLAM::Graph fg; - fg.addPoseConstraint(0, Pose2(0,0,0)); - fg.addRelativePose(0, 1, Pose2(1,2,M_PI_2), covariance); - - // Create initial config - Values initial; - initial.insert(0, Pose2(0,0,0)); - initial.insert(1, Pose2(0,0,0)); - - // Choose an ordering and optimize - Ordering ordering; - ordering += 0, 1; - - LevenbergMarquardtParams params; - params.relativeErrorTol = 1e-15; - params.ordering = ordering; - Values actual = LevenbergMarquardtOptimizer(fg, initial, params).optimize(); - - // Check with expected config - Values expected; - expected.insert(0, Pose2(0,0,0)); - expected.insert(1, Pose2(1,2,M_PI_2)); - CHECK(assert_equal(expected, actual)); - - // Check marginals - Marginals marginals = fg.marginals(actual); - // Matrix expectedP0 = Infinity, as we have a pose constraint !? - // Matrix actualP0 = marginals.marginalCovariance(0); - // EQUALITY(expectedP0, actualP0); - Matrix expectedP1 = cov; // the second pose really should have just the noise covariance - Matrix actualP1 = marginals.marginalCovariance(1); - EQUALITY(expectedP1, actualP1); -} - -/* ************************************************************************* */ -TEST_UNSAFE(Pose2SLAM, optimizeSPCG) { - - // create a Pose graph with one equality constraint and one measurement - pose2SLAM::Graph fg; - fg.addPosePrior(0, Pose2(0,0,0), noiseModel::Diagonal::Sigmas(Vector_(3,3.0,3.0,1.0))); - fg.addRelativePose(0, 1, Pose2(1,2,M_PI_2), covariance); - - // [Duy] For some unknown reason, SPCG needs this constraint to work. GaussNewton doesn't need this. - fg.addRelativePose(0, 1, Pose2(1,2,M_PI_2), noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1))); - - // Create initial config - Values initial; - initial.insert(0, Pose2(0,0,0)); - initial.insert(1, Pose2(0,0,0)); - - // Optimize using SPCG - Values actual = fg.optimizeSPCG(initial,0); - - // Try GaussNewton without the above constraint to see if the problem is underconstrained. Still works! - Values actual2 = GaussNewtonOptimizer(fg, initial).optimize(); - - // Check with expected config - Values expected; - expected.insert(0, Pose2(0,0,0)); - expected.insert(1, Pose2(1,2,M_PI_2)); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(expected, actual2)); -} - -/* ************************************************************************* */ -// test optimization with 3 poses, note we use plain Keys here, not symbols -TEST_UNSAFE(Pose2SLAM, optimizeThreePoses) { - - // Create a hexagon of poses - pose2SLAM::Values hexagon = pose2SLAM::Values::Circle(3,1.0); - Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1); - - // create a Pose graph with one equality constraint and one measurement - pose2SLAM::Graph fg; - fg.addPoseConstraint(0, p0); - Pose2 delta = p0.between(p1); - fg.addRelativePose(0, 1, delta, covariance); - fg.addRelativePose(1, 2, delta, covariance); - fg.addRelativePose(2, 0, delta, covariance); - - // Create initial config - pose2SLAM::Values initial; - initial.insertPose(0, p0); - initial.insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1))); - initial.insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1))); - - // Choose an ordering - Ordering ordering; - ordering += 0,1,2; - - // optimize - LevenbergMarquardtParams params; - params.relativeErrorTol = 1e-15; - params.ordering = ordering; - Values actual = LevenbergMarquardtOptimizer(fg, initial, params).optimize(); - - // Check with ground truth - CHECK(assert_equal((const Values&)hexagon, actual)); -} - -/* ************************************************************************* */ -// test optimization with 6 poses arranged in a hexagon and a loop closure -TEST_UNSAFE(Pose2SLAM, optimizeCircle) { - - // Create a hexagon of poses - pose2SLAM::Values hexagon = pose2SLAM::Values::Circle(6,1.0); - Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1); - - // create a Pose graph with one equality constraint and one measurement - pose2SLAM::Graph fg; - fg.addPoseConstraint(0, p0); - Pose2 delta = p0.between(p1); - fg.addRelativePose(0,1, delta, covariance); - fg.addRelativePose(1,2, delta, covariance); - fg.addRelativePose(2,3, delta, covariance); - fg.addRelativePose(3,4, delta, covariance); - fg.addRelativePose(4,5, delta, covariance); - fg.addRelativePose(5,0, delta, covariance); - - // Create initial config - pose2SLAM::Values initial; - initial.insertPose(0, p0); - initial.insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1))); - initial.insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1))); - initial.insertPose(3, hexagon.pose(3).retract(Vector_(3,-0.1, 0.1,-0.1))); - initial.insertPose(4, hexagon.pose(4).retract(Vector_(3, 0.1,-0.1, 0.1))); - initial.insertPose(5, hexagon.pose(5).retract(Vector_(3,-0.1, 0.1,-0.1))); - - // Choose an ordering - Ordering ordering; - ordering += 0,1,2,3,4,5; - - // optimize - LevenbergMarquardtParams params; - params.relativeErrorTol = 1e-15; - params.ordering = ordering; - Values actual = LevenbergMarquardtOptimizer(fg, initial, params).optimize(); - - // Check with ground truth - CHECK(assert_equal((const Values&)hexagon, actual)); - - // Check loop closure - CHECK(assert_equal(delta, actual.at(5).between(actual.at(0)))); - -// Pose2SLAMOptimizer myOptimizer("3"); - -// Matrix A1 = myOptimizer.a1(); -// LONGS_EQUAL(3, A1.rows()); -// LONGS_EQUAL(17, A1.cols()); // 7 + 7 + 3 -// -// Matrix A2 = myOptimizer.a2(); -// LONGS_EQUAL(3, A1.rows()); -// LONGS_EQUAL(7, A2.cols()); // 7 -// -// Vector b1 = myOptimizer.b1(); -// LONGS_EQUAL(9, b1.size()); // 3 + 3 + 3 -// -// Vector b2 = myOptimizer.b2(); -// LONGS_EQUAL(3, b2.size()); // 3 -// -// // Here, call matlab to -// // A=[A1;A2], b=[b1;b2] -// // R=qr(A1) -// // call pcg on A,b, with preconditioner R -> get x -// -// Vector x = myOptimizer.optimize(); -// LONGS_EQUAL(9, x.size()); // 3 + 3 + 3 -// -// myOptimizer.update(x); -// -// Values expected; -// expected.insert(0, Pose2(0.,0.,0.)); -// expected.insert(1, Pose2(1.,0.,0.)); -// expected.insert(2, Pose2(2.,0.,0.)); -// -// // Check with ground truth -// CHECK(assert_equal(expected, *myOptimizer.theta())); -} - -/* ************************************************************************* */ -TEST_UNSAFE(Pose2SLAM, optimize2) { -// Pose2SLAMOptimizer myOptimizer("100"); -// Matrix A1 = myOptimizer.a1(); -// Matrix A2 = myOptimizer.a2(); -// cout << "A1: " << A1.rows() << " " << A1.cols() << endl; -// cout << "A2: " << A2.rows() << " " << A2.cols() << endl; -// -// //cout << "error: " << myOptimizer.error() << endl; -// for(int i = 0; i<10; i++) { -// myOptimizer.linearize(); -// Vector x = myOptimizer.optimize(); -// myOptimizer.update(x); -// } -// //cout << "error: " << myOptimizer.error() << endl; -// CHECK(myOptimizer.error() < 1.); -} - -///* ************************************************************************* */ -// SL-NEEDED? TEST_UNSAFE(Pose2SLAM, findMinimumSpanningTree) { -// pose2SLAM::Graph G, T, C; -// G.addPoseConstraint(1, 2, Pose2(0.,0.,0.), I3); -// G.addPoseConstraint(1, 3, Pose2(0.,0.,0.), I3); -// G.addPoseConstraint(2, 3, Pose2(0.,0.,0.), I3); -// -// PredecessorMap tree = -// G.findMinimumSpanningTree(); -// CHECK(tree[1] == 1); -// CHECK(tree[2] == 1); -// CHECK(tree[3] == 1); -//} -// -///* ************************************************************************* */ -// SL-NEEDED? TEST_UNSAFE(Pose2SLAM, split) { -// pose2SLAM::Graph G, T, C; -// G.addPoseConstraint(1, 2, Pose2(0.,0.,0.), I3); -// G.addPoseConstraint(1, 3, Pose2(0.,0.,0.), I3); -// G.addPoseConstraint(2, 3, Pose2(0.,0.,0.), I3); -// -// PredecessorMap tree; -// tree.insert(1,2); -// tree.insert(2,2); -// tree.insert(3,2); -// -// G.split(tree, T, C); -// LONGS_EQUAL(2, T.size()); -// LONGS_EQUAL(1, C.size()); -//} - -using namespace pose2SLAM; - -/* ************************************************************************* */ -TEST_UNSAFE(Pose2Values, pose2Circle ) -{ - // expected is 4 poses tangent to circle with radius 1m - pose2SLAM::Values expected; - expected.insert(0, Pose2( 1, 0, M_PI_2)); - expected.insert(1, Pose2( 0, 1, - M_PI )); - expected.insert(2, Pose2(-1, 0, - M_PI_2)); - expected.insert(3, Pose2( 0, -1, 0 )); - - pose2SLAM::Values actual = pose2SLAM::Values::Circle(4,1.0); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST_UNSAFE(Pose2SLAM, expmap ) -{ - // expected is circle shifted to right - pose2SLAM::Values expected; - expected.insert(0, Pose2( 1.1, 0, M_PI_2)); - expected.insert(1, Pose2( 0.1, 1, - M_PI )); - expected.insert(2, Pose2(-0.9, 0, - M_PI_2)); - expected.insert(3, Pose2( 0.1, -1, 0 )); - - // Note expmap coordinates are in local coordinates, so shifting to right requires thought !!! - pose2SLAM::Values circle = pose2SLAM::Values::Circle(4,1.0); - Ordering ordering(*circle.orderingArbitrary()); - VectorValues delta(circle.dims(ordering)); - delta[ordering[0]] = Vector_(3, 0.0,-0.1,0.0); - delta[ordering[1]] = Vector_(3, -0.1,0.0,0.0); - delta[ordering[2]] = Vector_(3, 0.0,0.1,0.0); - delta[ordering[3]] = Vector_(3, 0.1,0.0,0.0); - pose2SLAM::Values actual = circle.retract(delta, ordering); - CHECK(assert_equal(expected,actual)); -} - -// Common measurement covariance -static SharedNoiseModel sigmas = noiseModel::Diagonal::Sigmas(Vector_(3,sx,sy,st)); - -/* ************************************************************************* */ -// Very simple test establishing Ax-b \approx z-h(x) -TEST_UNSAFE( Pose2Prior, error ) -{ - // Choose a linearization point - Pose2 p1(1, 0, 0); // robot at (1,0) - pose2SLAM::Values x0; - x0.insert(1, p1); - - // Create factor - PriorFactor factor(1, p1, sigmas); - - // Actual linearization - Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr linear = - boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); - - // Check error at x0, i.e. delta = zero ! - VectorValues delta(VectorValues::Zero(x0.dims(ordering))); - delta[ordering[1]] = zero(3); - Vector error_at_zero = Vector_(3,0.0,0.0,0.0); - CHECK(assert_equal(error_at_zero,factor.whitenedError(x0))); - CHECK(assert_equal(-error_at_zero,linear->error_vector(delta))); - - // Check error after increasing p2 - VectorValues addition(VectorValues::Zero(x0.dims(ordering))); - addition[ordering[1]] = Vector_(3, 0.1, 0.0, 0.0); - VectorValues plus = delta + addition; - pose2SLAM::Values x1 = x0.retract(plus, ordering); - Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! - CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); - CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); -} - -/* ************************************************************************* */ -// common Pose2Prior for tests below -static gtsam::Pose2 priorVal(2,2,M_PI_2); -static PriorFactor priorFactor(1, priorVal, sigmas); - -/* ************************************************************************* */ -// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector -// Hence i.e., b = approximates z-h(x0) = error_vector(x0) -LieVector hprior(const Pose2& p1) { - return LieVector(sigmas->whiten(priorFactor.evaluateError(p1))); -} - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2Prior, linearize ) -{ - // Choose a linearization point at ground truth - pose2SLAM::Values x0; - x0.insertPose(1, priorVal); - - // Actual linearization - Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr actual = - boost::dynamic_pointer_cast(priorFactor.linearize(x0, ordering)); - - // Test with numerical derivative - Matrix numericalH = numericalDerivative11(hprior, priorVal); - CHECK(assert_equal(numericalH,actual->getA(actual->find(ordering[1])))); -} - -/* ************************************************************************* */ -// Very simple test establishing Ax-b \approx z-h(x) -TEST_UNSAFE( Pose2Factor, error ) -{ - // Choose a linearization point - Pose2 p1; // robot at origin - Pose2 p2(1, 0, 0); // robot at (1,0) - pose2SLAM::Values x0; - x0.insertPose(1, p1); - x0.insertPose(2, p2); - - // Create factor - Pose2 z = p1.between(p2); - Pose2Factor factor(1, 2, z, covariance); - - // Actual linearization - Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr linear = - boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); - - // Check error at x0, i.e. delta = zero ! - VectorValues delta(x0.dims(ordering)); - delta[ordering[1]] = zero(3); - delta[ordering[2]] = zero(3); - Vector error_at_zero = Vector_(3,0.0,0.0,0.0); - CHECK(assert_equal(error_at_zero,factor.unwhitenedError(x0))); - CHECK(assert_equal(-error_at_zero, linear->error_vector(delta))); - - // Check error after increasing p2 - VectorValues plus = delta; - plus[ordering[2]] = Vector_(3, 0.1, 0.0, 0.0); - pose2SLAM::Values x1 = x0.retract(plus, ordering); - Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! - CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); - CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); -} - -/* ************************************************************************* */ -// common Pose2Factor for tests below -static Pose2 measured(2,2,M_PI_2); -static Pose2Factor factor(1,2,measured, covariance); - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2Factor, rhs ) -{ - // Choose a linearization point - Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) - Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4) - pose2SLAM::Values x0; - x0.insert(1,p1); - x0.insert(2,p2); - - // Actual linearization - Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr linear = - boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); - - // Check RHS - Pose2 hx0 = p1.between(p2); - CHECK(assert_equal(Pose2(2.1, 2.1, M_PI_2),hx0)); - Vector expected_b = Vector_(3, -0.1/sx, 0.1/sy, 0.0); - CHECK(assert_equal(expected_b,-factor.whitenedError(x0))); - CHECK(assert_equal(expected_b,linear->getb())); -} - -/* ************************************************************************* */ -// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector -// Hence i.e., b = approximates z-h(x0) = error_vector(x0) -LieVector h(const Pose2& p1,const Pose2& p2) { - return LieVector(covariance->whiten(factor.evaluateError(p1,p2))); -} - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2Factor, linearize ) -{ - // Choose a linearization point at ground truth - Pose2 p1(1,2,M_PI_2); - Pose2 p2(-1,4,M_PI); - pose2SLAM::Values x0; - x0.insert(1,p1); - x0.insert(2,p2); - - // expected linearization - Matrix expectedH1 = covariance->Whiten(Matrix_(3,3, - 0.0,-1.0,-2.0, - 1.0, 0.0,-2.0, - 0.0, 0.0,-1.0 - )); - Matrix expectedH2 = covariance->Whiten(Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0 - )); - Vector expected_b = Vector_(3, 0.0, 0.0, 0.0); - - // expected linear factor - Ordering ordering(*x0.orderingArbitrary()); - SharedDiagonal probModel1 = noiseModel::Unit::Create(3); - JacobianFactor expected(ordering[1], expectedH1, ordering[2], expectedH2, expected_b, probModel1); - - // Actual linearization - boost::shared_ptr actual = - boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); - CHECK(assert_equal(expected,*actual)); - - // Numerical do not work out because BetweenFactor is approximate ? - Matrix numericalH1 = numericalDerivative21(h, p1, p2); - CHECK(assert_equal(expectedH1,numericalH1)); - Matrix numericalH2 = numericalDerivative22(h, p1, p2); - CHECK(assert_equal(expectedH2,numericalH2)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp deleted file mode 100644 index 23367bd8a..000000000 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ /dev/null @@ -1,211 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testpose3SLAM.cpp - * @author Frank Dellaert - * @author Viorela Ila - **/ - -#include -#include -#include - -#include - -#include -#include -#include -using namespace boost; -using namespace boost::assign; - -#include - -using namespace std; -using namespace gtsam; - -// common measurement covariance -static noiseModel::Gaussian::shared_ptr covariance(noiseModel::Unit::Create(6)); - -const double tol=1e-5; - -/* ************************************************************************* */ -// test optimization with 6 poses arranged in a hexagon and a loop closure -TEST(Pose3Graph, optimizeCircle) { - - // Create a hexagon of poses - double radius = 10; - Values hexagon = pose3SLAM::Values::Circle(6,radius); - Pose3 gT0 = hexagon.at(0), gT1 = hexagon.at(1); - - // create a Pose graph with one equality constraint and one measurement - pose3SLAM::Graph fg; - fg.addPoseConstraint(0, gT0); - Pose3 _0T1 = gT0.between(gT1); // inv(gT0)*gT1 - double theta = M_PI/3.0; - CHECK(assert_equal(Pose3(Rot3::yaw(-theta),Point3(radius*sin(theta),-radius*cos(theta),0)),_0T1)); - fg.addRelativePose(0,1, _0T1, covariance); - fg.addRelativePose(1,2, _0T1, covariance); - fg.addRelativePose(2,3, _0T1, covariance); - fg.addRelativePose(3,4, _0T1, covariance); - fg.addRelativePose(4,5, _0T1, covariance); - fg.addRelativePose(5,0, _0T1, covariance); - - // Create initial config - Values initial; - initial.insert(0, gT0); - initial.insert(1, hexagon.at(1).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial.insert(2, hexagon.at(2).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial.insert(3, hexagon.at(3).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial.insert(4, hexagon.at(4).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial.insert(5, hexagon.at(5).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - - // Choose an ordering and optimize - Ordering ordering; - ordering += 0,1,2,3,4,5; - - Values actual = LevenbergMarquardtOptimizer(fg, initial, ordering).optimize(); - - // Check with ground truth - CHECK(assert_equal(hexagon, actual,1e-4)); - - // Check loop closure - CHECK(assert_equal(_0T1, actual.at(5).between(actual.at(0)),1e-5)); -} - -/* ************************************************************************* */ -TEST(Pose3Graph, partial_prior_height) { - typedef PartialPriorFactor Partial; - - // reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3) - - // height prior - single element interface - double exp_height = 5.0; - SharedDiagonal model = noiseModel::Unit::Create(1); - Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height)); - Partial height(1, 5, exp_height, model); - Matrix actA; - EXPECT(assert_equal(Vector_(1,-2.0), height.evaluateError(init, actA), tol)); - Matrix expA = Matrix_(1, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); - EXPECT(assert_equal(expA, actA)); - - pose3SLAM::Graph graph; - graph.add(height); - - Values values; - values.insert(1, init); - - // linearization - EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol); - - Values actual = LevenbergMarquardtOptimizer(graph, values).optimize(); - EXPECT(assert_equal(expected, actual.at(1), tol)); - EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); -} - -/* ************************************************************************* */ -TEST( Pose3Factor, error ) -{ - // Create example - Pose3 t1; // origin - Pose3 t2(Rot3::rodriguez(0.1,0.2,0.3),Point3(0,1,0)); - Pose3 z(Rot3::rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));; - - // Create factor - SharedNoiseModel I6(noiseModel::Unit::Create(6)); - BetweenFactor factor(1, 2, z, I6); - - // Create config - Values x; - x.insert(1,t1); - x.insert(2,t2); - - // Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2)) - Vector actual = factor.unwhitenedError(x); - Vector expected = z.localCoordinates(t1.between(t2)); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST(Pose3Graph, partial_prior_xy) { - typedef PartialPriorFactor Partial; - - // XY prior - full mask interface - Vector exp_xy = Vector_(2, 3.0, 4.0); - SharedDiagonal model = noiseModel::Unit::Create(2); - Pose3 init(Rot3(), Point3(1.0,-2.0, 3.0)), expected(Rot3(), Point3(3.0, 4.0, 3.0)); - vector mask; mask += 3, 4; - Partial priorXY(1, mask, exp_xy, model); - Matrix actA; - EXPECT(assert_equal(Vector_(2,-2.0,-6.0), priorXY.evaluateError(init, actA), tol)); - Matrix expA = Matrix_(2, 6, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0, 0.0); - EXPECT(assert_equal(expA, actA)); - - pose3SLAM::Graph graph; - graph.add(priorXY); - - Values values; - values.insert(1, init); - - Values actual = LevenbergMarquardtOptimizer(graph, values).optimize(); - EXPECT(assert_equal(expected, actual.at(1), tol)); - EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); -} - -// The world coordinate system has z pointing up, y north, x east -// The vehicle has X forward, Y right, Z down -Rot3 R1(Point3( 0, 1, 0), Point3( 1, 0, 0), Point3(0, 0, -1)); -Rot3 R2(Point3(-1, 0, 0), Point3( 0, 1, 0), Point3(0, 0, -1)); -Rot3 R3(Point3( 0,-1, 0), Point3(-1, 0, 0), Point3(0, 0, -1)); -Rot3 R4(Point3( 1, 0, 0), Point3( 0,-1, 0), Point3(0, 0, -1)); - -/* ************************************************************************* */ -TEST( Values, pose3Circle ) -{ - // expected is 4 poses tangent to circle with radius 1m - Values expected; - expected.insert(0, Pose3(R1, Point3( 1, 0, 0))); - expected.insert(1, Pose3(R2, Point3( 0, 1, 0))); - expected.insert(2, Pose3(R3, Point3(-1, 0, 0))); - expected.insert(3, Pose3(R4, Point3( 0,-1, 0))); - - Values actual = pose3SLAM::Values::Circle(4,1.0); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( Values, expmap ) -{ - Values expected; - expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0))); - expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0))); - expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0))); - expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0))); - - Ordering ordering(*expected.orderingArbitrary()); - VectorValues delta(expected.dims(ordering)); - delta.vector() = Vector_(24, - 0.0,0.0,0.0, 0.1, 0.0, 0.0, - 0.0,0.0,0.0, 0.1, 0.0, 0.0, - 0.0,0.0,0.0, 0.1, 0.0, 0.0, - 0.0,0.0,0.0, 0.1, 0.0, 0.0); - Values actual = pose3SLAM::Values::Circle(4,1.0).retract(delta, ordering); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testSparseBA.cpp b/gtsam/slam/tests/testSparseBA.cpp deleted file mode 100644 index cc6e11400..000000000 --- a/gtsam/slam/tests/testSparseBA.cpp +++ /dev/null @@ -1,193 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testsparseBA.cpp - * @brief - * @date Jul 5, 2012 - * @author Yong-Dian Jian - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace boost; -using namespace gtsam; - -/* ************************************************************************* */ - -static SharedNoiseModel sigma(noiseModel::Unit::Create(2)); - -// Convenience for named keys -using symbol_shorthand::X; /* pose3 */ -using symbol_shorthand::K; /* calibration */ -using symbol_shorthand::C; /* camera = [pose calibration] */ -using symbol_shorthand::L; /* point3 */ - -static Point3 landmark1(-1.0,-1.0, 0.0); -static Point3 landmark2(-1.0, 1.0, 0.0); -static Point3 landmark3( 1.0, 1.0, 0.0); -static Point3 landmark4( 1.0,-1.0, 0.0); - -static Pose3 pose1(Matrix_(3,3, - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1.), - Point3(0,0,6.25)); - -static Pose3 pose2(Matrix_(3,3, - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1.), - Point3(0,0,5.00)); - -static Cal3_S2 calib1 (625, 625, 0, 0, 0); -static Cal3_S2 calib2 (625, 625, 0, 0, 0); - - -typedef PinholeCamera Camera; - -static Camera camera1(pose1, calib1); -static Camera camera2(pose2, calib2); - -/* ************************************************************************* */ -sparseBA::Graph testGraph1() { - Point2 z11(-100, 100); - Point2 z12(-100,-100); - Point2 z13( 100,-100); - Point2 z14( 100, 100); - Point2 z21(-125, 125); - Point2 z22(-125,-125); - Point2 z23( 125,-125); - Point2 z24( 125, 125); - - - sparseBA::Graph g; - g.addMeasurement(z11, sigma, C(1), L(1)); - g.addMeasurement(z12, sigma, C(1), L(2)); - g.addMeasurement(z13, sigma, C(1), L(3)); - g.addMeasurement(z14, sigma, C(1), L(4)); - g.addMeasurement(z21, sigma, C(2), L(1)); - g.addMeasurement(z22, sigma, C(2), L(2)); - g.addMeasurement(z23, sigma, C(2), L(3)); - g.addMeasurement(z24, sigma, C(2), L(4)); - return g; -} - -sparseBA::Graph testGraph2() { - Point2 z11(-100, 100); - Point2 z12(-100,-100); - Point2 z13( 100,-100); - Point2 z14( 100, 100); - Point2 z21(-125, 125); - Point2 z22(-125,-125); - Point2 z23( 125,-125); - Point2 z24( 125, 125); - - sparseBA::Graph g; - g.addMeasurement(z11, sigma, X(1), L(1), K(1)); - g.addMeasurement(z12, sigma, X(1), L(2), K(1)); - g.addMeasurement(z13, sigma, X(1), L(3), K(1)); - g.addMeasurement(z14, sigma, X(1), L(4), K(1)); - g.addMeasurement(z21, sigma, X(2), L(1), K(1)); - g.addMeasurement(z22, sigma, X(2), L(2), K(1)); - g.addMeasurement(z23, sigma, X(2), L(3), K(1)); - g.addMeasurement(z24, sigma, X(2), L(4), K(1)); - return g; -} - -/* ************************************************************************* */ -TEST( optimizeLM1, sparseBA ) -{ - // build a graph - sparseBA::Graph graph(testGraph1()); - - // add 3 landmark constraints - graph.addPointConstraint(L(1), landmark1); - graph.addPointConstraint(L(2), landmark2); - graph.addPointConstraint(L(3), landmark3); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(C(1), camera1); - initialEstimate.insert(C(2), camera2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - - // Create an ordering of the variables - Ordering ordering; - ordering += L(1),L(2),L(3),L(4),C(1),C(2); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); -} - -/* ************************************************************************* */ -TEST( optimizeLM2, sparseBA ) -{ - // build a graph - sparseBA::Graph graph(testGraph2()); - - // add 3 landmark constraints - graph.addPointConstraint(L(1), landmark1); - graph.addPointConstraint(L(2), landmark2); - graph.addPointConstraint(L(3), landmark3); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(X(1), pose1); - initialEstimate.insert(X(2), pose2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - initialEstimate.insert(K(1), calib2); - - // Create an ordering of the variables - Ordering ordering; - ordering += L(1),L(2),L(3),L(4),X(1),X(2),K(1); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testVisualSLAM.cpp b/gtsam/slam/tests/testVisualSLAM.cpp deleted file mode 100644 index 65c31adf9..000000000 --- a/gtsam/slam/tests/testVisualSLAM.cpp +++ /dev/null @@ -1,344 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testVisualSLAM.cpp - * @brief Unit test for two cameras and four landmarks, single camera - * @author Chris Beall - * @author Frank Dellaert - * @author Viorela Ila - */ - -#include -#include -#include -#include - -#include -#include -using namespace boost; - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ - -static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::L; - -static Point3 landmark1(-1.0,-1.0, 0.0); -static Point3 landmark2(-1.0, 1.0, 0.0); -static Point3 landmark3( 1.0, 1.0, 0.0); -static Point3 landmark4( 1.0,-1.0, 0.0); - -static Pose3 pose1(Matrix_(3,3, - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ), - Point3(0,0,6.25)); - -static Pose3 pose2(Matrix_(3,3, - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ), - Point3(0,0,5.00)); - -/* ************************************************************************* */ -visualSLAM::Graph testGraph() { - Point2 z11(-100, 100); - Point2 z12(-100,-100); - Point2 z13( 100,-100); - Point2 z14( 100, 100); - Point2 z21(-125, 125); - Point2 z22(-125,-125); - Point2 z23( 125,-125); - Point2 z24( 125, 125); - - shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0)); - visualSLAM::Graph g; - g.addMeasurement(z11, sigma, X(1), L(1), sK); - g.addMeasurement(z12, sigma, X(1), L(2), sK); - g.addMeasurement(z13, sigma, X(1), L(3), sK); - g.addMeasurement(z14, sigma, X(1), L(4), sK); - g.addMeasurement(z21, sigma, X(2), L(1), sK); - g.addMeasurement(z22, sigma, X(2), L(2), sK); - g.addMeasurement(z23, sigma, X(2), L(3), sK); - g.addMeasurement(z24, sigma, X(2), L(4), sK); - return g; -} - -/* ************************************************************************* */ -TEST( VisualSLAM, optimizeLM) -{ - // build a graph - visualSLAM::Graph graph(testGraph()); - // add 3 landmark constraints - graph.addPointConstraint(L(1), landmark1); - graph.addPointConstraint(L(2), landmark2); - graph.addPointConstraint(L(3), landmark3); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(X(1), pose1); - initialEstimate.insert(X(2), pose2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - - // Create an ordering of the variables - Ordering ordering; - ordering += L(1),L(2),L(3),L(4),X(1),X(2); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); -} - - -/* ************************************************************************* */ -TEST( VisualSLAM, optimizeLM2) -{ - // build a graph - visualSLAM::Graph graph(testGraph()); - // add 2 camera constraints - graph.addPoseConstraint(X(1), pose1); - graph.addPoseConstraint(X(2), pose2); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(X(1), pose1); - initialEstimate.insert(X(2), pose2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - - // Create an ordering of the variables - Ordering ordering; - ordering += L(1),L(2),L(3),L(4),X(1),X(2); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, LMoptimizer) -{ - // build a graph - visualSLAM::Graph graph(testGraph()); - // add 2 camera constraints - graph.addPoseConstraint(X(1), pose1); - graph.addPoseConstraint(X(2), pose2); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(X(1), pose1); - initialEstimate.insert(X(2), pose2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer = graph.optimizer(initialEstimate); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); - - // check errors - Matrix errors = graph.reprojectionErrors(optimizer.values()); - CHECK(assert_equal(zeros(2,8), errors)); -} - - -/* ************************************************************************* */ -TEST( VisualSLAM, CHECK_ORDERING) -{ - // build a graph - visualSLAM::Graph graph = testGraph(); - // add 2 camera constraints - graph.addPoseConstraint(X(1), pose1); - graph.addPoseConstraint(X(2), pose2); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(X(1), pose1); - initialEstimate.insert(X(2), pose2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, update_with_large_delta) { - // this test ensures that if the update for delta is larger than - // the size of the config, it only updates existing variables - Values init; - init.insert(X(1), Pose3()); - init.insert(L(1), Point3(1.0, 2.0, 3.0)); - - Values expected; - expected.insert(X(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); - expected.insert(L(1), Point3(1.1, 2.1, 3.1)); - - Ordering largeOrdering; - Values largeValues = init; - largeValues.insert(X(2), Pose3()); - largeOrdering += X(1),L(1),X(2); - VectorValues delta(largeValues.dims(largeOrdering)); - delta[largeOrdering[X(1)]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1); - delta[largeOrdering[L(1)]] = Vector_(3, 0.1, 0.1, 0.1); - delta[largeOrdering[X(2)]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1); - Values actual = init.retract(delta, largeOrdering); - - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, dataAssociation) { - visualSLAM::ISAM isam; - // add two landmarks - // add two cameras and constraint and odometry -// std::pair actualJoint = isam.jointPrediction(); // 4*4 - // std::pair actualMarginals = isam.individualPredictions(); // 2 times 2*2 - // std::pair actualChowLiu = isam.chowLiuPredictions(); // 2 times 2*2 -} - -/* ************************************************************************* */ -TEST( VisualSLAM, filteredValues) { - visualSLAM::Values full; - full.insert(X(1), Pose3(Pose2(1.0, 2.0, 0.3))); - full.insert(L(1), Point3(1.0, 2.0, 3.0)); - - visualSLAM::Values actPoses(full.allPoses()); - visualSLAM::Values expPoses; expPoses.insert(X(1), Pose3(Pose2(1.0, 2.0, 0.3))); - EXPECT(assert_equal(expPoses, actPoses)); - - visualSLAM::Values actPoints(full.allPoints()); - visualSLAM::Values expPoints; expPoints.insert(L(1), Point3(1.0, 2.0, 3.0)); - EXPECT(assert_equal(expPoints, actPoints)); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, keys_and_view ) -{ - // create config - visualSLAM::Values c; - c.insert(X(1), pose1); - c.insert(X(2), pose2); - c.insert(L(2), landmark2); - LONGS_EQUAL(2,c.nrPoses()); - LONGS_EQUAL(1,c.nrPoints()); - { - FastList expected, actual; - expected += L(2), X(1), X(2); - actual = c.keys(); - CHECK(expected == actual); - } - { - FastList expected, actual; - expected += X(1), X(2); - actual = c.poseKeys(); - CHECK(expected == actual); - } - { - FastList expected, actual; - expected += L(2); - actual = c.pointKeys(); - CHECK(expected == actual); - } -} - -/* ************************************************************************* */ -TEST( VisualSLAM, addMeasurements ) -{ - // create config - visualSLAM::Graph g; - Vector J = Vector_(3,1.0,2.0,3.0); - Matrix Z = Matrix_(2,3, -1.0,0.0,1.0, -1.0,0.0,1.0); - shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0)); - g.addMeasurements(0,J,Z,sigma,sK); - EXPECT_LONGS_EQUAL(3,g.size()); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, insertBackProjections ) -{ - // create config - visualSLAM::Values c; - SimpleCamera camera(pose1); - Vector J = Vector_(3,1.0,2.0,3.0); - Matrix Z = Matrix_(2,3, -1.0,0.0,1.0, -1.0,0.0,1.0); - c.insertBackprojections(camera,J,Z,1.0); - EXPECT_LONGS_EQUAL(3,c.nrPoints()); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, perturbPoints ) -{ - visualSLAM::Values c1,c2; - c1.insert(L(2), landmark2); - c1.perturbPoints(0.01,42u); - CHECK(assert_equal(Point3(-0.986984, 0.999534, -0.0147962),c1.point(L(2)),1e-6)); - c2.insert(L(2), landmark2); - c2.perturbPoints(0.01,42u); - CHECK(assert_equal(Point3(-0.986984, 0.999534, -0.0147962),c2.point(L(2)),1e-6)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp deleted file mode 100644 index 2133c0ee8..000000000 --- a/gtsam/slam/visualSLAM.cpp +++ /dev/null @@ -1,116 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file visualSLAM.cpp - * @date Jan 14, 2010 - * @author richard - */ - -#include -#include -#include -#include -#include - -namespace visualSLAM { - - using boost::make_shared; - - /* ************************************************************************* */ - void Values::insertBackprojections(const SimpleCamera& camera, - const Vector& J, const Matrix& Z, double depth) { - if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); - if (Z.cols() != J.size()) throw std::invalid_argument( - "insertBackProjections: J and Z must have same number of entries"); - for(int k=0;k points = allPoints(); - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); - Sampler sampler(model, seed); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, points) { - update(keyValue.key, keyValue.value.retract(sampler.sample())); - } - } - - /* ************************************************************************* */ - Matrix Values::points() const { - size_t j=0; - ConstFiltered points = allPoints(); - Matrix result(points.size(),3); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, points) - result.row(j++) = keyValue.value.vector(); - return result; - } - - /* ************************************************************************* */ - void Graph::addPointConstraint(Key pointKey, const Point3& p) { - push_back(make_shared >(pointKey, p)); - } - - /* ************************************************************************* */ - void Graph::addPointPrior(Key pointKey, const Point3& p, const SharedNoiseModel& model) { - push_back(make_shared >(pointKey, p, model)); - } - - /* ************************************************************************* */ - void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model, - Key poseKey, Key pointKey, const shared_ptrK K) { - push_back( - make_shared > - (measured, model, poseKey, pointKey, K)); - } - - /* ************************************************************************* */ - void Graph::addMeasurements(Key i, const Vector& J, const Matrix& Z, - const SharedNoiseModel& model, const shared_ptrK K) { - if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K"); - if (Z.cols() != J.size()) throw std::invalid_argument( - "addMeasurements: J and Z must have same number of entries"); - for (int k = 0; k < Z.cols(); k++) - addMeasurement(Point2(Z(0, k), Z(1, k)), model, i, J(k), K); - } - - /* ************************************************************************* */ - void Graph::addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model, - Key poseKey, Key pointKey, const shared_ptrKStereo K) { - push_back(make_shared >(measured, model, poseKey, pointKey, K)); - } - - /* ************************************************************************* */ - void Graph::addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model) { - push_back(make_shared >(poseKey, pointKey, range, model)); - } - - /* ************************************************************************* */ - Matrix Graph::reprojectionErrors(const Values& values) const { - // first count - size_t K = 0, k=0; - BOOST_FOREACH(const sharedFactor& f, *this) - if (boost::dynamic_pointer_cast(f)) ++K; - // now fill - Matrix errors(2,K); - BOOST_FOREACH(const sharedFactor& f, *this) { - boost::shared_ptr p = - boost::dynamic_pointer_cast(f); - if (p) errors.col(k++) = p->unwhitenedError(values); - } - return errors; - } - /* ************************************************************************* */ -} diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h deleted file mode 100644 index ae2956c5f..000000000 --- a/gtsam/slam/visualSLAM.h +++ /dev/null @@ -1,186 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file visualSLAM.h - * @brief Basic typedefs for general VisualSLAM problems. Useful for monocular and stereo systems. - * @date Jan 14, 2010 - * @author Richard Roberts - * @author Chris Beall - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace visualSLAM { - - using namespace gtsam; - - /// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper - struct Values: public pose3SLAM::Values { - - typedef boost::shared_ptr shared_ptr; - typedef gtsam::Values::ConstFiltered PoseFiltered; - typedef gtsam::Values::ConstFiltered PointFiltered; - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const gtsam::Values& values) : - pose3SLAM::Values(values) { - } - - /// Constructor from filtered values view of poses - Values(const PoseFiltered& view) : pose3SLAM::Values(view) {} - - /// Constructor from filtered values view of points - Values(const PointFiltered& view) : pose3SLAM::Values(view) {} - - PoseFiltered allPoses() const { return this->filter(); } ///< pose view - size_t nrPoses() const { return allPoses().size(); } ///< get number of poses - KeyList poseKeys() const { return allPoses().keys(); } ///< get keys to poses only - - PointFiltered allPoints() const { return this->filter(); } ///< point view - size_t nrPoints() const { return allPoints().size(); } ///< get number of points - KeyList pointKeys() const { return allPoints().keys(); } ///< get keys to points only - - /// insert a point - void insertPoint(Key j, const Point3& point) { insert(j, point); } - - /// update a point - void updatePoint(Key j, const Point3& point) { update(j, point); } - - /// get a point - Point3 point(Key j) const { return at(j); } - - /// insert a number of initial point values by backprojecting - void insertBackprojections(const SimpleCamera& c, const Vector& J, - const Matrix& Z, double depth); - - /// perturb all points using normally distributed noise - void perturbPoints(double sigma, int32_t seed = 42u); - - /// get all point coordinates in a matrix - Matrix points() const; - - }; - - /** - * Non-linear factor graph for vanilla visual SLAM - */ - struct Graph: public pose3SLAM::Graph { - - /// shared pointer to this type of graph - typedef boost::shared_ptr shared_graph; - - /// Default constructor - Graph(){} - - /// Copy constructor given any other NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph): - pose3SLAM::Graph(graph) {} - - /// check if two graphs are equal - bool equals(const Graph& p, double tol = 1e-9) const { - return NonlinearFactorGraph::equals(p, tol); - } - - /** - * Add a constraint on a point (for now, *must* be satisfied in any Values) - * @param key variable key of the point - * @param p point around which soft prior is defined - */ - void addPointConstraint(Key pointKey, const Point3& p = Point3()); - - /** - * Add a prior on a point - * @param key variable key of the point - * @param p to which point to constrain it to - * @param model uncertainty model of this prior - */ - void addPointPrior(Key pointKey, const Point3& p = Point3(), - const SharedNoiseModel& model = noiseModel::Unit::Create(3)); - - /** - * Add a range prior to a point - * @param poseKey variable key of the camera pose - * @param pointKey variable key of the point - * @param range approximate range to point - * @param model uncertainty model of this prior - */ - void addRangeFactor(Key poseKey, Key pointKey, double range, - const SharedNoiseModel& model = noiseModel::Unit::Create(1)); - - /** - * Add a projection factor measurement (monocular) - * @param measured the measurement - * @param model the noise model for the measurement - * @param poseKey variable key for the camera pose - * @param pointKey variable key for the point - * @param K shared pointer to calibration object - */ - void addMeasurement(const Point2& measured, const SharedNoiseModel& model, - Key poseKey, Key pointKey, const shared_ptrK K); - - /** - * Add a number of measurements at the same time - * @param i variable key for the camera pose - * @param J variable keys for the point, KeyVector of size K - * @param Z the 2*K measurements as a matrix - * @param model the noise model for the measurement - * @param K shared pointer to calibration object - */ - void addMeasurements(Key i, const Vector& J, const Matrix& Z, - const SharedNoiseModel& model, const shared_ptrK K); - - /** - * Add a stereo factor measurement - * @param measured the measurement - * @param model the noise model for the measurement - * @param poseKey variable key for the camera pose - * @param pointKey variable key for the point - * @param K shared pointer to stereo calibration object - */ - void addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model, - Key poseKey, Key pointKey, const shared_ptrKStereo K); - - /// Return a 2*K Matrix of reprojection errors - Matrix reprojectionErrors(const Values& values) const; - - }; // Graph - - /** - * Non-linear ISAM for vanilla incremental visual SLAM inference - */ - typedef gtsam::NonlinearISAM ISAM; - -} // visualSLAM - -/** - * Backwards compatibility and wrap use only, avoid using - */ -namespace visualSLAM { - typedef gtsam::NonlinearEquality PoseConstraint; ///< \deprecated typedef for backwards compatibility - typedef gtsam::NonlinearEquality PointConstraint; ///< \deprecated typedef for backwards compatibility - typedef gtsam::PriorFactor PosePrior; ///< \deprecated typedef for backwards compatibility - typedef gtsam::PriorFactor PointPrior; ///< \deprecated typedef for backwards compatibility - typedef gtsam::RangeFactor RangeFactor; ///< \deprecated typedef for backwards compatibility - typedef gtsam::GenericProjectionFactor ProjectionFactor; ///< \deprecated typedef for backwards compatibility - typedef gtsam::GenericStereoFactor StereoFactor; ///< \deprecated typedef for backwards compatibility -} -