diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 642e2350b..eb3e2f761 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -33,33 +33,6 @@ namespace gtsam { -/** - * Structure for storing some state memory, used to speed up optimization - * @addtogroup SLAM - */ -class SmartStereoProjectionFactorState { - -protected: - -public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - SmartStereoProjectionFactorState() { - } - // Hessian representation (after Schur complement) - bool calculatedHessian; - Matrix H; - Vector gs_vector; - std::vector Gs; - std::vector gs; - double f; -}; - -enum LinearizationMode { - HESSIAN, JACOBIAN_SVD, JACOBIAN_Q -}; - /** * SmartStereoProjectionFactor: triangulates point */ @@ -84,14 +57,6 @@ protected: mutable bool degenerate_; mutable bool cheiralityException_; - // verbosity handling for Cheirality Exceptions - const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) - const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) - - boost::shared_ptr state_; - - /// shorthand for smart projection factor state variable - typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type typedef SmartFactorBase Base; @@ -110,6 +75,12 @@ protected: ZDim = 3 }; ///< Dimension trait of measurement type + /// @name Parameters governing how triangulation result is treated + /// @{ + const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) + const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + /// @} + public: /// shorthand for a smart pointer to a factor @@ -133,12 +104,11 @@ public: SmartStereoProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state = - SmartFactorStatePtr(new SmartStereoProjectionFactorState())) : + double dynamicOutlierRejectionThreshold = -1) : rankTolerance_(rankTol), retriangulationThreshold_(1e-5), manageDegeneracy_( manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( - false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_( + false), verboseCheirality_(false), landmarkDistanceThreshold_( landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_( dynamicOutlierRejectionThreshold) { } @@ -199,40 +169,6 @@ public: return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation } - /// This function checks if the new linearization point_ is 'close' to the previous one used for linearization - bool decideIfLinearize(const Cameras& cameras) const { - // "selective linearization" - // The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose - // (we only care about the "rigidity" of the poses, not about their absolute pose) - - if (this->linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize - return true; - - // if we do not have a previous linearization point_ or the new linearization point_ includes more poses - if (cameraPosesLinearization_.empty() - || (cameras.size() != cameraPosesLinearization_.size())) - return true; - - Pose3 firstCameraPose, firstCameraPoseOld; - for (size_t i = 0; i < cameras.size(); i++) { - - if (i == 0) { // we store the initial pose, this is useful for selective re-linearization - firstCameraPose = cameras[i].pose(); - firstCameraPoseOld = cameraPosesLinearization_[i]; - continue; - } - - // we compare the poses in the frame of the first pose - Pose3 localCameraPose = firstCameraPose.between(cameras[i].pose()); - Pose3 localCameraPoseOld = firstCameraPoseOld.between( - cameraPosesLinearization_[i]); - if (!localCameraPose.equals(localCameraPoseOld, - this->linearizationThreshold_)) - return true; // at least two "relative" poses are different, hence we re-linearize - } - return false; // if we arrive to this point_ all poses are the same and we don't need re-linearize - } - /// triangulateSafe size_t triangulateSafe(const Values& values) const { return triangulateSafe(this->cameras(values)); @@ -364,7 +300,7 @@ public: exit(1); } - this->triangulateSafe(cameras); + triangulateSafe(cameras); if (isDebug) std::cout << "point_ = " << point_ << std::endl; @@ -388,29 +324,10 @@ public: std::cout << "degenerate_ = true" << std::endl; } - bool doLinearize = this->decideIfLinearize(cameras); - - if (isDebug) - std::cout << "doLinearize = " << doLinearize << std::endl; - - if (this->linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize + if (this->linearizationThreshold_ >= 0) // if we apply selective relinearization and we need to relinearize for (size_t i = 0; i < cameras.size(); i++) this->cameraPosesLinearization_[i] = cameras[i].pose(); - if (!doLinearize) { // return the previous Hessian factor - std::cout << "=============================" << std::endl; - std::cout << "doLinearize " << doLinearize << std::endl; - std::cout << "this->linearizationThreshold_ " - << this->linearizationThreshold_ << std::endl; - std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; - std::cout - << "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled" - << std::endl; - exit(1); - return boost::make_shared >(this->keys_, - this->state_->Gs, this->state_->gs, this->state_->f); - } - // ================================================================== std::vector Fblocks; Matrix F, E; @@ -447,11 +364,6 @@ public: } // ================================================================== double f = b.squaredNorm(); - if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables - this->state_->Gs = Gs; - this->state_->gs = gs; - this->state_->f = f; - } return boost::make_shared >(this->keys_, Gs, gs, f); } diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 2ac719056..a9cec0f57 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -40,6 +40,13 @@ namespace gtsam { */ template class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { + +public: + /// Linearization mode: what factor to linearize to + enum LinearizationMode { + HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD + }; + protected: LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 0112db915..258c8d0eb 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -64,7 +64,6 @@ 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; vector stereo_projectToMultipleCameras(const StereoCamera& cam1, const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { @@ -327,15 +326,15 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { cam2, cam3, landmark3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD)); smartFactor1->add(measurements_cam1, views, model, K); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD)); smartFactor2->add(measurements_cam2, views, model, K); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -395,17 +394,17 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { cam2, cam3, landmark3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor1->add(measurements_cam1, views, model, K); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor2->add(measurements_cam2, views, model, K); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model, K); @@ -473,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, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor1->add(measurements_cam1, views, model, K); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor2->add(measurements_cam2, views, model, K); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor3->add(measurements_cam3, views, model, K); SmartFactor::shared_ptr smartFactor4( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor4->add(measurements_cam4, views, model, K);