diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index f4e82d98d..8b655af46 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -33,6 +33,12 @@ namespace gtsam { +/// Linearization mode: what factor to linearize to + enum LinearizationMode { + HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD + }; + + /** * SmartStereoProjectionFactor: triangulates point */ @@ -82,11 +88,8 @@ public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - /// shorthand for a StereoCamera // TODO: Get rid of this? - typedef StereoCamera Camera; - /// Vector of cameras - typedef CameraSet Cameras; + typedef CameraSet Cameras; /** * Constructor @@ -195,7 +198,7 @@ public: } std::vector > mono_cameras; - BOOST_FOREACH(const Camera& camera, cameras) { + BOOST_FOREACH(const StereoCamera& camera, cameras) { const Pose3& pose = camera.pose(); const Cal3_S2& K = camera.calibration()->calibration(); mono_cameras.push_back(PinholeCamera(pose, K)); @@ -215,7 +218,7 @@ public: // Check landmark distance and reprojection errors to avoid outliers double totalReprojError = 0.0; size_t i = 0; - BOOST_FOREACH(const Camera& camera, cameras) { + BOOST_FOREACH(const StereoCamera& camera, cameras) { Point3 cameraTranslation = camera.pose().translation(); // we discard smart factors corresponding to points that are far away if (cameraTranslation.distance(point_) > parameters_.landmarkDistanceThreshold) { @@ -578,9 +581,6 @@ public: } } - /// Cameras are computed in derived class - virtual Cameras cameras(const Values& values) const = 0; - /** return the landmark */ boost::optional point() const { return point_; diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index bc4d3ccfb..ac269335e 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -42,10 +42,7 @@ template class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { public: - /// Linearization mode: what factor to linearize to - enum LinearizationMode { - HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD - }; + protected: @@ -163,7 +160,7 @@ public: size_t i=0; BOOST_FOREACH(const Key& k, this->keys_) { Pose3 pose = values.at(k); - typename Base::Camera camera(pose, K_all_[i++]); + StereoCamera camera(pose, K_all_[i++]); cameras.push_back(camera); } return cameras; diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 258c8d0eb..90e88c666 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -326,15 +326,15 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { cam2, cam3, landmark3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); smartFactor1->add(measurements_cam1, views, model, K); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); smartFactor2->add(measurements_cam2, views, model, K); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -394,17 +394,17 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { cam2, cam3, landmark3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor1->add(measurements_cam1, views, model, K); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor2->add(measurements_cam2, views, model, K); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model, K); @@ -472,22 +472,22 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor1->add(measurements_cam1, views, model, K); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor2->add(measurements_cam2, views, model, K); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor3->add(measurements_cam3, views, model, K); SmartFactor::shared_ptr smartFactor4( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor4->add(measurements_cam4, views, model, K);