diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index b2076cc14..b01420446 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -71,8 +71,6 @@ class SmartProjectionFactorP : public SmartProjectionFactor { public: typedef CAMERA Camera; typedef CameraSet Cameras; - typedef Eigen::Matrix MatrixZD; // F blocks - typedef std::vector > FBlocks; // vector of F blocks /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -234,7 +232,8 @@ class SmartProjectionFactorP : public SmartProjectionFactor { * respect to both body poses we interpolate from), the point Jacobian E, * and the error vector b. Note that the jacobians are computed for a given point. */ - void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b, + void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks& Fs, + Matrix& E, Vector& b, const Cameras& cameras) const { if (!this->result_) { throw("computeJacobiansWithTriangulatedPoint"); @@ -289,7 +288,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { } // compute Jacobian given triangulated 3D Point - FBlocks Fs; + typename Base::FBlocks Fs; Matrix E; Vector b; this->computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras); @@ -300,7 +299,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { Fs[i] = this->noiseModel_->Whiten(Fs[i]); } - Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); + const Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); // Build augmented Hessian (with last row/column being the information vector) // Note: we need to get the augumented hessian wrt the unique keys in key_