diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 7c5abca53..69f92e402 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -25,10 +25,9 @@ #include "RegularHessianFactor.h" #include -#include +#include // for Cheirality exception #include #include -#include #include #include diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index 56e5fdbf1..f93fb468b 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -408,44 +408,44 @@ public: return boost::make_shared >(this->keys_, Gs, gs, f); } - // create factor - boost::shared_ptr > createImplicitSchurFactor( - const Cameras& cameras, double lambda) const { - if (triangulateForLinearize(cameras)) - return Base::createImplicitSchurFactor(cameras, point_, lambda); - else - return boost::shared_ptr >(); - } - - /// create factor - 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_); - } - - /// Create a factor, takes values - boost::shared_ptr > createJacobianQFactor( - const Values& values, double lambda) const { - Cameras myCameras; - // TODO triangulate twice ?? - bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); - if (nonDegenerate) - return createJacobianQFactor(myCameras, lambda); - else - return boost::make_shared< JacobianFactorQ >(this->keys_); - } - - /// different (faster) way to compute Jacobian factor - boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, - double lambda) const { - if (triangulateForLinearize(cameras)) - return Base::createJacobianSVDFactor(cameras, point_, lambda); - else - return boost::make_shared< JacobianFactorSVD >(this->keys_); - } +// // create factor +// boost::shared_ptr > createImplicitSchurFactor( +// const Cameras& cameras, double lambda) const { +// if (triangulateForLinearize(cameras)) +// return Base::createImplicitSchurFactor(cameras, point_, lambda); +// else +// return boost::shared_ptr >(); +// } +// +// /// create factor +// 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_); +// } +// +// /// Create a factor, takes values +// boost::shared_ptr > createJacobianQFactor( +// const Values& values, double lambda) const { +// Cameras myCameras; +// // TODO triangulate twice ?? +// bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); +// if (nonDegenerate) +// return createJacobianQFactor(myCameras, lambda); +// else +// return boost::make_shared< JacobianFactorQ >(this->keys_); +// } +// +// /// different (faster) way to compute Jacobian factor +// boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, +// double lambda) const { +// if (triangulateForLinearize(cameras)) +// return Base::createJacobianSVDFactor(cameras, point_, lambda); +// else +// return boost::make_shared< JacobianFactorSVD >(this->keys_); +// } /// Returns true if nonDegenerate bool computeCamerasAndTriangulate(const Values& values, @@ -546,41 +546,41 @@ public: } // end else } - /// Version that computes PointCov, with optional lambda parameter - double computeJacobians(std::vector& Fblocks, - Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras, - const double lambda = 0.0) const { - - double f = computeJacobians(Fblocks, E, b, cameras); - - // Point covariance inv(E'*E) - PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse(); - - return f; - } - - /// takes values - bool computeJacobiansSVD(std::vector& Fblocks, - Matrix& Enull, Vector& b, const Values& values) const { - typename Base::Cameras myCameras; - double good = computeCamerasAndTriangulate(values, myCameras); - if (good) - computeJacobiansSVD(Fblocks, Enull, b, myCameras); - return true; - } - - /// SVD version - double computeJacobiansSVD(std::vector& Fblocks, - Matrix& Enull, Vector& b, const Cameras& cameras) const { - return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); - } - - /// Returns Matrix, TODO: maybe should not exist -> not sparse ! - // TODO should there be a lambda? - double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, - const Cameras& cameras) const { - return Base::computeJacobiansSVD(F, Enull, b, cameras, point_); - } +// /// Version that computes PointCov, with optional lambda parameter +// double computeJacobians(std::vector& Fblocks, +// Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras, +// const double lambda = 0.0) const { +// +// double f = computeJacobians(Fblocks, E, b, cameras); +// +// // Point covariance inv(E'*E) +// PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse(); +// +// return f; +// } +// +// /// takes values +// bool computeJacobiansSVD(std::vector& Fblocks, +// Matrix& Enull, Vector& b, const Values& values) const { +// typename Base::Cameras myCameras; +// double good = computeCamerasAndTriangulate(values, myCameras); +// if (good) +// computeJacobiansSVD(Fblocks, Enull, b, myCameras); +// return true; +// } +// +// /// SVD version +// double computeJacobiansSVD(std::vector& Fblocks, +// Matrix& Enull, Vector& b, const Cameras& cameras) const { +// return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); +// } +// +// /// Returns Matrix, TODO: maybe should not exist -> not sparse ! +// // TODO should there be a lambda? +// double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, +// const Cameras& cameras) const { +// return Base::computeJacobiansSVD(F, Enull, b, cameras, point_); +// } /// Returns Matrix, TODO: maybe should not exist -> not sparse ! double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, diff --git a/gtsam/slam/SmartStereoProjectionPoseFactor.h b/gtsam/slam/SmartStereoProjectionPoseFactor.h index 06e82b2a7..4077071d9 100644 --- a/gtsam/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam/slam/SmartStereoProjectionPoseFactor.h @@ -172,12 +172,12 @@ public: const Values& values) const { // depending on flag set on construction we may linearize to different linear factors switch(linearizeTo_){ - case JACOBIAN_SVD : - return this->createJacobianSVDFactor(cameras(values), 0.0); - break; - case JACOBIAN_Q : - return this->createJacobianQFactor(cameras(values), 0.0); - break; +// case JACOBIAN_SVD : +// return this->createJacobianSVDFactor(cameras(values), 0.0); +// break; +// case JACOBIAN_Q : +// return this->createJacobianQFactor(cameras(values), 0.0); +// break; default: return this->createHessianFactor(cameras(values)); break;