diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index f282682b3..b1f8dc6a6 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -358,23 +358,6 @@ VectorValues HessianFactor::hessianDiagonal() const { return d; } -/* ************************************************************************* */ -// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n -void HessianFactor::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]; - // Get the diagonal block, and insert its diagonal - const Matrix& B = info_(pos, pos).selfadjointView(); - DMap(d + 9 * j) += B.diagonal(); - } -} - /* ************************************************************************* */ map HessianFactor::hessianBlockDiagonal() const { map blocks; @@ -548,48 +531,6 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, } } -/* ************************************************************************* */ -void HessianFactor::multiplyHessianAdd(double alpha, const double* x, - double* yvalues, 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 - vector y; - y.reserve(size()); - for (const_iterator it = begin(); it != end(); it++) - y.push_back(zero(getDim(it))); - - // 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 - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - DenseIndex i = 0; - for (; i < j; ++i) - y[i] += info_(i, j).knownOffDiagonal() - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); - // blocks on the diagonal are only half - y[i] += info_(j, j).selfadjointView() - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); - // for below diagonal, we take transpose block from upper triangular part - for (i = j + 1; i < (DenseIndex) size(); ++i) - y[i] += info_(i, j).knownOffDiagonal() - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); - } - - // copy to yvalues - for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) - DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) += - alpha * y[i]; -} - - /* ************************************************************************* */ VectorValues HessianFactor::gradientAtZero() const { VectorValues g; @@ -599,23 +540,6 @@ VectorValues HessianFactor::gradientAtZero() const { return g; } -/* ************************************************************************* */ -// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n -void HessianFactor::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 - DVector dj = -info_(pos,size()).knownOffDiagonal(); - DMap(d + 9 * j) += dj; - } -} - /* ************************************************************************* */ std::pair, boost::shared_ptr > EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 62d84925c..44118cece 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -340,9 +340,6 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const; - /* ************************************************************************* */ - virtual void hessianDiagonal(double* d) const; - /// Return the block diagonal of the Hessian for this factor virtual std::map hessianBlockDiagonal() const; @@ -380,15 +377,9 @@ namespace gtsam { /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; - void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const; - - void multiplyHessianAdd(double alpha, const double* x, double* y) const {}; - /// eta for Hessian VectorValues gradientAtZero() const; - virtual void gradientAtZero(double* d) const; - /** * Densely partially eliminate with Cholesky factorization. JacobianFactors are * left-multiplied with their transpose to form the Hessian using the conversion constructor