diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index c272eac8e..5765f67fd 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -10,10 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file RegularHessianFactor.h - * @brief HessianFactor class with constant sized blcoks - * @author Richard Roberts - * @date Dec 8, 2010 + * @file RegularHessianFactor.h + * @brief HessianFactor class with constant sized blocks + * @author Sungtae An + * @date March 2014 */ #pragma once @@ -29,8 +29,10 @@ class RegularHessianFactor: public HessianFactor { private: - typedef Eigen::Matrix MatrixDD; // camera hessian block - typedef Eigen::Matrix VectorD; + // Use Eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; public: @@ -61,7 +63,6 @@ public: } // Scratch space for multiplyHessianAdd - typedef Eigen::Matrix DVector; mutable std::vector y; /** y += alpha * A'*A*x */ @@ -78,9 +79,6 @@ public: BOOST_FOREACH(DVector & yi, y) yi.setZero(); - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - // Accessing the VectorValues one by one is expensive // So we will loop over columns to access x only once per column // And fill the above temporary y values, to be added into yvalues after @@ -109,11 +107,6 @@ public: void multiplyHessianAdd(double alpha, const double* x, double* yvalues, std::vector offsets) const { - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - // Create a vector of temporary y values, corresponding to rows i std::vector y; y.reserve(size()); @@ -149,10 +142,6 @@ public: /** Return the diagonal of the Hessian for this factor (raw memory version) */ virtual void hessianDiagonal(double* d) const { - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - // Loop over all variables in the factor for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { Key j = keys_[pos]; @@ -165,22 +154,19 @@ public: /// Add gradient at zero to d TODO: is it really the goal to add ?? virtual void gradientAtZero(double* d) const { - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - // Loop over all variables in the factor for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { Key j = keys_[pos]; // Get the diagonal block, and insert its diagonal - VectorD dj = -info_(pos, size()).knownOffDiagonal(); + DVector dj = -info_(pos, size()).knownOffDiagonal(); DMap(d + D * j) += dj; } } /* ************************************************************************* */ -}; // end class RegularHessianFactor +}; +// end class RegularHessianFactor // traits template struct traits > : public Testable<