diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index 92e7d92bc..f3f90dc2d 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -51,16 +51,11 @@ public: HessianFactor(keys, augmentedInformation) { } - /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const VectorValues& x, - VectorValues& y) const { - HessianFactor::multiplyHessianAdd(alpha, x, y); - } - // Scratch space for multiplyHessianAdd typedef Eigen::Matrix DVector; mutable std::vector y; + /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const { // Create a vector of temporary y values, corresponding to rows i y.resize(size()); @@ -134,16 +129,12 @@ public: alpha * y[i]; } - /** Return the diagonal of the Hessian for this factor */ - VectorValues hessianDiagonal() const { - return HessianFactor::hessianDiagonal(); - } - /** Return the diagonal of the Hessian for this factor (raw memory version) */ - void hessianDiagonal(double* d) const { + virtual void hessianDiagonal(double* d) const { // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; + //typedef Eigen::Matrix DVector; + typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; // Loop over all variables in the factor @@ -151,20 +142,18 @@ public: 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(); + //DMap(d + 9 * j) += B.diagonal(); + DMap(d + D * j) += B.diagonal(); } } - VectorValues gradientAtZero() const { - return HessianFactor::gradientAtZero(); - } - /* ************************************************************************* */ // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n - void gradientAtZero(double* d) const { + virtual void gradientAtZero(double* d) const { // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; + //typedef Eigen::Matrix DVector; + typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; // Loop over all variables in the factor @@ -172,7 +161,8 @@ public: Key j = keys_[pos]; // Get the diagonal block, and insert its diagonal VectorD dj = -info_(pos,size()).knownOffDiagonal(); - DMap(d + 9 * j) += dj; + //DMap(d + 9 * j) += dj; + DMap(d + D * j) += dj; } } diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index f1a11e2cd..21508587e 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -160,7 +160,7 @@ public: * @brief add the contribution of this factor to the diagonal of the hessian * d(output) = d(input) + deltaHessianFactor */ - void hessianDiagonal(double* d) const { + virtual void hessianDiagonal(double* d) const { // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); // Use eigen magic to access raw memory typedef Eigen::Matrix DVector; @@ -435,7 +435,7 @@ public: /** * Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS */ - void gradientAtZero(double* d) const { + virtual void gradientAtZero(double* d) const { // Use eigen magic to access raw memory typedef Eigen::Matrix DVector; diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index 51a4a27a1..a9759cc26 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -57,13 +57,7 @@ public: JacobianFactor(keys, augmentedMatrix, sigmas) { } - /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const VectorValues& x, - VectorValues& y) const { - JacobianFactor::multiplyHessianAdd(alpha, x, y); - } - - /** Raw memory access version of multiplyHessianAdd + /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x * Note: this is not assuming a fixed dimension for the variables, * but requires the vector accumulatedDims to tell the dimension of * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, @@ -126,16 +120,10 @@ public: DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax; } -// /// Return the diagonal of the Hessian for this factor -// VectorValues hessianDiagonal() const { -// return JacobianFactor::hessianDiagonal(); -// } - /** Raw memory access version of hessianDiagonal * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n - * */ - void hessianDiagonal(double* d) const { + virtual void hessianDiagonal(double* d) const { // Loop over all variables in the factor for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert its diagonal @@ -153,22 +141,18 @@ public: } } - /** // Gradient is really -A'*b / sigma^2 */ - VectorValues gradientAtZero() const { - return JacobianFactor::gradientAtZero(); - } - /** Raw memory access version of gradientAtZero */ - void gradientAtZero(double* d) const { + virtual void gradientAtZero(double* d) const { // Get vector b not weighted Vector b = getb(); Vector b_sigma = model_ ? (model_->whiten(b)*model_->whiten(b)) : b; - // gradient -= A'*b/sigma^2 - // Loop over all variables in the factor + // Just iterate over all A matrices for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - // Get the diagonal block, and insert its diagonal + DMap(d + D * j) = DVector::Zero(); DVector dj; + // gradient -= A'*b/sigma^2 + // Computing with each column for (size_t k = 0; k < D; ++k){ Vector column_k = Ab_(j).col(k); dj(k) = -1.0*dot(b_sigma,column_k); diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp index 14de545a6..7e7f877f4 100644 --- a/gtsam/slam/tests/testRegularHessianFactor.cpp +++ b/gtsam/slam/tests/testRegularHessianFactor.cpp @@ -77,15 +77,6 @@ TEST(RegularHessianFactor, ConstructorNWay) expected.insert(1, Y.segment<2>(2)); expected.insert(3, Y.segment<2>(4)); - // VectorValues way - VectorValues actual; - factor.multiplyHessianAdd(1, x, actual); - EXPECT(assert_equal(expected, actual)); - - // now, do it with non-zero y - factor.multiplyHessianAdd(1, x, actual); - EXPECT(assert_equal(2*expected, actual)); - // RAW ACCESS Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696; Vector fast_y = gtsam::zero(8); diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp index efa603aca..c19f6db8f 100644 --- a/gtsam/slam/tests/testRegularJacobianFactor.cpp +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -98,12 +98,6 @@ TEST(RegularJacobianFactor, hessianDiagonal) // we compute hessian diagonal from the standard Jacobian VectorValues expectedHessianDiagonal = factor.hessianDiagonal(); - // we compute hessian diagonal from the regular Jacobian, but still using the - // implementation of hessianDiagonal in the base class - //VectorValues actualHessianDiagonal = regularFactor.hessianDiagonal(); - - //EXPECT(assert_equal(expectedHessianDiagonal,actualHessianDiagonal)); - // we compare against the Raw memory access implementation of hessianDiagonal double actualValue[9]; regularFactor.hessianDiagonal(actualValue); @@ -122,18 +116,13 @@ TEST(RegularJacobian, gradientAtZero) // we compute gradient at zero from the standard Jacobian VectorValues expectedGradientAtZero = factor.gradientAtZero(); - // we compute gradient at zero from the regular Jacobian, but still using the - // implementation of gradientAtZero in the base class - VectorValues actualGradientAtZero = regularFactor.gradientAtZero(); - - EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero())); + //EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero())); // we compare against the Raw memory access implementation of gradientAtZero double actualValue[9]; regularFactor.gradientAtZero(actualValue); VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim); EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw)); - } /* ************************************************************************* */ @@ -161,11 +150,6 @@ TEST(RegularJacobian, multiplyHessianAdd) VectorValues expectedMHA = Y; factor.multiplyHessianAdd(alpha, X, expectedMHA); - VectorValues actualMHA = Y; - regularFactor.multiplyHessianAdd(alpha, X, actualMHA); - - EXPECT(assert_equal(expectedMHA, actualMHA)); - // create data for raw memory access double XRaw[9]; vv2double(X, XRaw, nrKeys, fixedDim);