diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index b3c5ee5fe..eed389df9 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -61,8 +61,7 @@ using namespace std; using namespace gtsam; // Make the typename short so it looks much cleaner -typedef gtsam::SmartProjectionPoseFactor SmartFactor; +typedef gtsam::SmartProjectionPoseFactor SmartFactor; /* ************************************************************************* */ int main(int argc, char* argv[]) { diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index e0eac6b66..8ff554104 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -48,7 +48,7 @@ protected: boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) - static const int ZDim = traits::dimension::value; ///< Dimension trait of measurement type + static const int ZDim = traits::dimension::value; ///< Measurement dimension /// Definitions for blocks of F typedef Eigen::Matrix Matrix2D; // F diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 8ade075af..75e4699d9 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -61,7 +61,7 @@ enum LinearizationMode { * SmartProjectionFactor: triangulates point * TODO: why LANDMARK parameter? */ -template +template class SmartProjectionFactor: public SmartFactorBase, D> { protected: @@ -102,9 +102,9 @@ protected: // and the factor is disregarded if the error is large /// shorthand for this class - typedef SmartProjectionFactor This; + typedef SmartProjectionFactor This; - typedef traits::dimension ZDim_t; ///< Dimension trait of measurement type + static const int ZDim = traits::dimension::value; ///< Measurement dimension public: @@ -420,16 +420,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 +437,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 +446,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,4 +709,7 @@ private: } }; +template +const int SmartProjectionFactor::ZDim; + } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 0fd82414a..3b2e2bcbc 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; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 4e4fde3e4..c2855f09b 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); + SmartProjectionPoseFactor 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/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index 01c2ab3e1..c8a4e7123 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -46,7 +46,7 @@ using namespace gtsam; int main(int argc, char** argv){ - typedef SmartProjectionPoseFactor SmartFactor; + typedef SmartProjectionPoseFactor SmartFactor; Values initial_estimate; NonlinearFactorGraph graph;