diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 197ee82d2..642e2350b 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -63,7 +63,7 @@ enum LinearizationMode { /** * SmartStereoProjectionFactor: triangulates point */ -template +template class SmartStereoProjectionFactor: public SmartFactorBase { protected: @@ -104,7 +104,7 @@ protected: // and the factor is disregarded if the error is large /// shorthand for this class - typedef SmartStereoProjectionFactor This; + typedef SmartStereoProjectionFactor This; enum { ZDim = 3 @@ -347,7 +347,7 @@ public: } /// linearize returns a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( + boost::shared_ptr > createHessianFactor( const Cameras& cameras, const double lambda = 0.0) const { bool isDebug = false; @@ -374,10 +374,10 @@ public: if (isDebug) std::cout << "In linearize: exception" << std::endl; BOOST_FOREACH(Matrix& m, Gs) - m = zeros(D, D); + m = zeros(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) - v = zero(D); - return boost::make_shared >(this->keys_, Gs, gs, + v = zero(Base::Dim); + return boost::make_shared >(this->keys_, Gs, gs, 0.0); } @@ -407,7 +407,7 @@ public: << "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled" << std::endl; exit(1); - return boost::make_shared >(this->keys_, + return boost::make_shared >(this->keys_, this->state_->Gs, this->state_->gs, this->state_->f); } @@ -421,7 +421,7 @@ public: // Schur complement trick // Frank says: should be possible to do this more efficiently? // And we care, as in grouped factors this is called repeatedly - Matrix H(D * numKeys, D * numKeys); + Matrix H(Base::Dim * numKeys, Base::Dim * numKeys); Vector gs_vector; Matrix3 P = Cameras::PointCov(E, lambda); @@ -436,11 +436,11 @@ public: // Populate Gs and gs int GsCount2 = 0; for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera - DenseIndex i1D = i1 * D; - gs.at(i1) = gs_vector.segment(i1D); + DenseIndex i1D = i1 * Base::Dim; + gs.at(i1) = gs_vector.segment(i1D); for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) { if (i2 >= i1) { - Gs.at(GsCount2) = H.block(i1D, i2 * D); + Gs.at(GsCount2) = H.block(i1D, i2 * Base::Dim); GsCount2++; } } @@ -452,29 +452,29 @@ public: this->state_->gs = gs; this->state_->f = f; } - return boost::make_shared >(this->keys_, Gs, gs, f); + return boost::make_shared >(this->keys_, Gs, gs, f); } // // create factor -// boost::shared_ptr > createImplicitSchurFactor( +// boost::shared_ptr > createImplicitSchurFactor( // const Cameras& cameras, double lambda) const { // if (triangulateForLinearize(cameras)) // return Base::createImplicitSchurFactor(cameras, point_, lambda); // else -// return boost::shared_ptr >(); +// return boost::shared_ptr >(); // } // // /// 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 cameras; // // TODO triangulate twice ?? @@ -482,7 +482,7 @@ public: // if (nonDegenerate) // return createJacobianQFactor(cameras, lambda); // else -// return boost::make_shared< JacobianFactorQ >(this->keys_); +// return boost::make_shared< JacobianFactorQ >(this->keys_); // } // /// different (faster) way to compute Jacobian factor @@ -491,7 +491,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared >(this->keys_); + return boost::make_shared >(this->keys_); } /// Returns true if nonDegenerate @@ -717,9 +717,9 @@ private: }; /// traits -template -struct traits > : public Testable< - SmartStereoProjectionFactor > { +template +struct traits > : public Testable< + SmartStereoProjectionFactor > { }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 8fe8bea69..2ac719056 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -39,7 +39,7 @@ namespace gtsam { * @addtogroup SLAM */ template -class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { +class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { protected: LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) @@ -51,7 +51,7 @@ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for base class type - typedef SmartStereoProjectionFactor Base; + typedef SmartStereoProjectionFactor Base; /// shorthand for this class typedef SmartStereoProjectionPoseFactor This;