From 8a64d5bffe5560e1169cffe18bdcbd5e66736b06 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Feb 2015 12:27:40 +0100 Subject: [PATCH] Got rid of template parameters POSE (which was a fiction) and Z (which is now derived as CAMERA::Measurement). --- gtsam.h | 10 +++---- gtsam/slam/SmartFactorBase.h | 15 +++++----- gtsam/slam/SmartProjectionFactor.h | 30 ++++++++----------- gtsam/slam/SmartProjectionPoseFactor.h | 16 +++++----- gtsam/slam/tests/testSmartFactorBase.cpp | 17 +++++------ .../tests/testSmartProjectionPoseFactor.cpp | 6 ++-- .../slam/SmartStereoProjectionFactor.h | 19 ++++++------ .../slam/SmartStereoProjectionPoseFactor.h | 16 +++++----- .../testSmartStereoProjectionPoseFactor.cpp | 4 +-- 9 files changed, 64 insertions(+), 69 deletions(-) diff --git a/gtsam.h b/gtsam.h index 2aa20c278..43bcd86be 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2301,23 +2301,23 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { }; #include -template +template virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { SmartProjectionPoseFactor(double rankTol, double linThreshold, - bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); + bool manageDegeneracy, bool enableEPI, const gtsam::Pose3& body_P_sensor); SmartProjectionPoseFactor(double rankTol); SmartProjectionPoseFactor(); - void add(const gtsam::Point2& measured_i, size_t poseKey_i, const gtsam::noiseModel::Base* noise_i, - const CALIBRATION* K_i); + void add(const gtsam::Point2& measured_i, size_t poseKey_i, + const gtsam::noiseModel::Base* noise_i, const CALIBRATION* K_i); // enabling serialization functionality //void serialize() const; }; -typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; +typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; #include diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index ec5bed1b0..3bb66e57f 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -42,7 +42,7 @@ namespace gtsam { * The methods take a Cameras argument, which should behave like PinholeCamera, and * the value of a point, which is kept in the base class. */ -template +template class SmartFactorBase: public NonlinearFactor { protected: @@ -53,10 +53,11 @@ protected: * 2D measurement and noise model for each of the m views * The order is kept the same as the keys that we use to create the factor. */ + typedef typename CAMERA::Measurement Z; std::vector measured_; std::vector noise_; ///< noise model used - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) static const int ZDim = traits::dimension; ///< Measurement dimension @@ -73,7 +74,7 @@ protected: typedef NonlinearFactor Base; /// shorthand for this class - typedef SmartFactorBase This; + typedef SmartFactorBase This; public: @@ -90,7 +91,7 @@ public: * Constructor * @param body_P_sensor is the transform from sensor to body frame (default identity) */ - SmartFactorBase(boost::optional body_P_sensor = boost::none) : + SmartFactorBase(boost::optional body_P_sensor = boost::none) : body_P_sensor_(body_P_sensor) { } @@ -279,7 +280,7 @@ public: * F is a vector of derivatives wrpt the cameras, and E the stacked derivatives * with respect to the point. The value of cameras/point are passed as parameters. * Given a Point3, assumes dimensionality is 3. - * TODO We assume below the dimensionality of POSE is 6. Frank thinks the templating + * TODO We assume below the dimensionality of Pose3 is 6. Frank thinks the templating * of this factor is only for show, and should just assume a PinholeCamera. */ double computeJacobians(std::vector& Fblocks, Matrix& E, @@ -731,7 +732,7 @@ private: } }; -template -const int SmartFactorBase::ZDim; +template +const int SmartFactorBase::ZDim; } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 2bfcbfaa0..6b86b0b1e 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -58,11 +58,10 @@ enum LinearizationMode { }; /** - * SmartProjectionFactor: triangulates point - * TODO: why LANDMARK parameter? + * SmartProjectionFactor: triangulates point and keeps an estimate of it around. */ -template -class SmartProjectionFactor: public SmartFactorBase, D> { +template +class SmartProjectionFactor: public SmartFactorBase, D> { protected: // Some triangulation parameters @@ -92,7 +91,7 @@ protected: typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase, D> Base; + typedef SmartFactorBase, D> Base; double landmarkDistanceThreshold_; // if the landmark is triangulated at a // distance larger than that the factor is considered degenerate @@ -102,9 +101,7 @@ protected: // and the factor is disregarded if the error is large /// shorthand for this class - typedef SmartProjectionFactor This; - - static const int ZDim = 2; ///< Measurement dimension + typedef SmartProjectionFactor This; public: @@ -126,7 +123,7 @@ public: */ SmartProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, - boost::optional body_P_sensor = boost::none, + boost::optional body_P_sensor = boost::none, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : @@ -338,7 +335,7 @@ public: || (!this->manageDegeneracy_ && (this->cheiralityException_ || this->degenerate_))) { // std::cout << "In linearize: exception" << std::endl; - BOOST_FOREACH(gtsam::Matrix& m, Gs) + BOOST_FOREACH(Matrix& m, Gs) m = zeros(D, D); BOOST_FOREACH(Vector& v, gs) v = zero(D); @@ -420,16 +417,16 @@ public: } /// create factor - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createJacobianQFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared< JacobianFactorQ >(this->keys_); } /// Create a factor, takes values - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Values& values, double lambda) const { Cameras myCameras; // TODO triangulate twice ?? @@ -437,7 +434,7 @@ public: if (nonDegenerate) return createJacobianQFactor(myCameras, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared< JacobianFactorQ >(this->keys_); } /// different (faster) way to compute Jacobian factor @@ -446,7 +443,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + return boost::make_shared< JacobianFactorSVD >(this->keys_); } /// Returns true if nonDegenerate @@ -709,7 +706,4 @@ private: } }; -template -const int SmartProjectionFactor::ZDim; - } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index ac0951e87..70476ba3e 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -37,8 +37,8 @@ namespace gtsam { * The calibration is known here. The factor only constraints poses (variable dimension is 6) * @addtogroup SLAM */ -template -class SmartProjectionPoseFactor: public SmartProjectionFactor { +template +class SmartProjectionPoseFactor: public SmartProjectionFactor { protected: LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) @@ -48,10 +48,10 @@ protected: public: /// shorthand for base class type - typedef SmartProjectionFactor Base; + typedef SmartProjectionFactor Base; /// shorthand for this class - typedef SmartProjectionPoseFactor This; + typedef SmartProjectionPoseFactor This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -67,7 +67,7 @@ public: */ SmartProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + const bool enableEPI = false, boost::optional body_P_sensor = boost::none, LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1) : Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, @@ -211,7 +211,9 @@ private: }; // end of class declaration /// traits -template -struct traits > : public Testable > {}; +template +struct traits > : public Testable< + SmartProjectionPoseFactor > { +}; } // \ namespace gtsam diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index 618644981..b5ef18842 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -26,8 +26,7 @@ using namespace gtsam; /* ************************************************************************* */ #include #include -class PinholeFactor: public SmartFactorBase, 9> { +class PinholeFactor: public SmartFactorBase, 9> { virtual double error(const Values& values) const { return 0.0; } @@ -39,14 +38,14 @@ class PinholeFactor: public SmartFactorBase -class StereoFactor: public SmartFactorBase { +class StereoFactor: public SmartFactorBase { virtual double error(const Values& values) const { return 0.0; } @@ -58,9 +57,9 @@ class StereoFactor: public SmartFactorBase TEST(SmartFactorBase, Stereo) { StereoFactor f; - f.add(StereoPoint2(),1,SharedNoiseModel()); - f.add(StereoPoint2(),2,SharedNoiseModel()); - EXPECT_LONGS_EQUAL(2*3,f.dim()); + f.add(StereoPoint2(), 1, SharedNoiseModel()); + f.add(StereoPoint2(), 2, SharedNoiseModel()); + EXPECT_LONGS_EQUAL(2 * 3, f.dim()); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 6a79a331f..99df9f273 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -60,8 +60,8 @@ static Key poseKey1(x1); static Point2 measurement1(323.0, 240.0); static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); -typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionPoseFactor SmartFactorBundler; +typedef SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionPoseFactor SmartFactorBundler; void projectToMultipleCameras( SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark, @@ -1202,7 +1202,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){ /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { - SmartProjectionPoseFactor factor1(rankTol, linThreshold); + SmartFactorBundler factor1(rankTol, linThreshold); boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); factor1.add(measurement1, poseKey1, model, Kbundler); } diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 5d2ba3323..aeefe6c58 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -62,10 +62,9 @@ enum LinearizationMode { /** * SmartStereoProjectionFactor: triangulates point - * TODO: why LANDMARK parameter? */ -template -class SmartStereoProjectionFactor: public SmartFactorBase { +template +class SmartStereoProjectionFactor: public SmartFactorBase { protected: // Some triangulation parameters @@ -95,7 +94,7 @@ protected: typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase Base; + typedef SmartFactorBase Base; double landmarkDistanceThreshold_; // if the landmark is triangulated at a // distance larger than that the factor is considered degenerate @@ -105,7 +104,7 @@ protected: // and the factor is disregarded if the error is large /// shorthand for this class - typedef SmartStereoProjectionFactor This; + typedef SmartStereoProjectionFactor This; enum {ZDim = 3}; ///< Dimension trait of measurement type @@ -131,7 +130,7 @@ public: */ SmartStereoProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, - boost::optional body_P_sensor = boost::none, + boost::optional body_P_sensor = boost::none, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state = SmartFactorStatePtr(new SmartStereoProjectionFactorState())) : @@ -370,7 +369,7 @@ public: || (!this->manageDegeneracy_ && (this->cheiralityException_ || this->degenerate_))) { if (isDebug) std::cout << "In linearize: exception" << std::endl; - BOOST_FOREACH(gtsam::Matrix& m, Gs) + BOOST_FOREACH(Matrix& m, Gs) m = zeros(D, D); BOOST_FOREACH(Vector& v, gs) v = zero(D); @@ -746,9 +745,9 @@ private: }; /// traits -template -struct traits > : - public Testable > { +template +struct traits > : + public Testable > { }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 6a6d55988..3e0c6476f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -37,8 +37,8 @@ namespace gtsam { * The calibration is known here. The factor only constraints poses (variable dimension is 6) * @addtogroup SLAM */ -template -class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { +template +class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { protected: LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) @@ -50,10 +50,10 @@ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for base class type - typedef SmartStereoProjectionFactor Base; + typedef SmartStereoProjectionFactor Base; /// shorthand for this class - typedef SmartStereoProjectionPoseFactor This; + typedef SmartStereoProjectionPoseFactor This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -69,7 +69,7 @@ public: */ SmartStereoProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + const bool enableEPI = false, boost::optional body_P_sensor = boost::none, LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1) : Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, @@ -211,9 +211,9 @@ private: }; // end of class declaration /// traits -template -struct traits > : - public Testable > { +template +struct traits > : public Testable< + SmartStereoProjectionPoseFactor > { }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 4691da384..bab9712c2 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -62,8 +62,8 @@ static Key poseKey1(x1); static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value? static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); -typedef SmartStereoProjectionPoseFactor SmartFactor; -typedef SmartStereoProjectionPoseFactor SmartFactorBundler; +typedef SmartStereoProjectionPoseFactor SmartFactor; +typedef SmartStereoProjectionPoseFactor SmartFactorBundler; vector stereo_projectToMultipleCameras( const StereoCamera& cam1, const StereoCamera& cam2,