diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index fad49a339..160395997 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -36,6 +36,16 @@ namespace gtsam { +/// Linearization mode: what factor to linearize to +enum LinearizationMode { + HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD +}; + +/// How to manage degeneracy +enum DegeneracyMode { + IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY +}; + /** * @brief Base class for smart factors * This base class has no internal point, but it has a measurement, noise model diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 64b300b86..b1e2870dd 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -31,16 +31,6 @@ namespace gtsam { -/// Linearization mode: what factor to linearize to -enum LinearizationMode { - HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD -}; - -/// How to manage degeneracy -enum DegeneracyMode { - IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY -}; - /* * Parameters for the smart projection factors */ diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index e2b35ad82..778829765 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -35,17 +35,7 @@ namespace gtsam { -/// Linearization mode: what factor to linearize to - enum LinearizationMode { - HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD - }; - -/// How to manage degeneracy -enum DegeneracyMode { - IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY - }; - - /* + /* * Parameters for the smart stereo projection factors */ struct GTSAM_EXPORT SmartStereoProjectionParams { @@ -257,7 +247,7 @@ public: const StereoPoint2 zi = measured_[i]; monoCameras.push_back(leftCamera_i); monoMeasured.push_back(Point2(zi.uL(),zi.v())); - if(!isnan(zi.uR())){ // if right point is valid + if(!std::isnan(zi.uR())){ // if right point is valid monoCameras.push_back(rightCamera_i); monoMeasured.push_back(Point2(zi.uR(),zi.v())); } diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index c58a81dae..3acfb85de 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -18,7 +18,7 @@ * @date Sept 2013 */ -// TODO #include +#include #include #include #include @@ -33,8 +33,6 @@ using namespace boost::assign; using namespace gtsam; // make a realistic calibration matrix -static double fov = 60; // degrees -static size_t w = 640, h = 480; static double b = 1; static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b));