From 5e568bc29db748b080ded69a892635808e874a7e Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 18 Feb 2015 10:09:20 +0100 Subject: [PATCH] Restored VectorValues versions, they work fine --- .cproject | 4 +-- gtsam/slam/RegularHessianFactor.h | 27 +++++++++---------- gtsam/slam/RegularJacobianFactor.h | 6 +++++ gtsam/slam/tests/testRegularHessianFactor.cpp | 22 +++++++++------ 4 files changed, 34 insertions(+), 25 deletions(-) diff --git a/.cproject b/.cproject index a974f16d4..d2323e19c 100644 --- a/.cproject +++ b/.cproject @@ -1538,10 +1538,10 @@ true true - + make -j4 - testRegularJacobianFactor.run + testRegularHessianFactor.run true true true diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index 8c3df87cf..dbe374958 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -56,13 +56,14 @@ public: mutable std::vector y; /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const{ - throw std::runtime_error( - "RegularHessianFactor::forbidden use of multiplyHessianAdd without raw memory access, use HessianFactor instead"); + virtual void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + HessianFactor::multiplyHessianAdd(alpha, x, y); } /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const { + void multiplyHessianAdd(double alpha, const double* x, + double* yvalues) const { // Create a vector of temporary y values, corresponding to rows i y.resize(size()); BOOST_FOREACH(DVector & yi, y) @@ -95,6 +96,7 @@ public: } } + /// Raw memory version, with offsets TODO document reasoning void multiplyHessianAdd(double alpha, const double* x, double* yvalues, std::vector offsets) const { @@ -131,43 +133,38 @@ public: // 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]; + DMap(yvalues + offsets[keys_[i]], + offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y[i]; } /** 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::Matrix DVector; typedef Eigen::Map DMap; // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + 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(); DMap(d + D * j) += B.diagonal(); } } - /* ************************************************************************* */ - // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n + /// 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::Matrix DVector; typedef Eigen::Map DMap; // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + 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(); - //DMap(d + 9 * j) += dj; + VectorD dj = -info_(pos, size()).knownOffDiagonal(); DMap(d + D * j) += dj; } } diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index 97b2ca460..dcf709854 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -57,6 +57,12 @@ public: JacobianFactor(keys, augmentedMatrix, sigmas) { } + /** y += alpha * A'*A*x */ + virtual void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + JacobianFactor::multiplyHessianAdd(alpha, x, y); + } + /** 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 diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp index 8dfb753f4..e2b8ac3cf 100644 --- a/gtsam/slam/tests/testRegularHessianFactor.cpp +++ b/gtsam/slam/tests/testRegularHessianFactor.cpp @@ -15,11 +15,10 @@ * @date March 4, 2014 */ -#include -#include - -//#include #include +#include + +#include #include #include @@ -29,8 +28,6 @@ using namespace std; using namespace gtsam; using namespace boost::assign; -const double tol = 1e-5; - /* ************************************************************************* */ TEST(RegularHessianFactor, ConstructorNWay) { @@ -77,15 +74,24 @@ TEST(RegularHessianFactor, ConstructorNWay) expected.insert(1, Y.segment<2>(2)); expected.insert(3, Y.segment<2>(4)); + // VectorValues version + double alpha = 1.0; + VectorValues actualVV; + actualVV.insert(0, zero(2)); + actualVV.insert(1, zero(2)); + actualVV.insert(3, zero(2)); + factor.multiplyHessianAdd(alpha, x, actualVV); + EXPECT(assert_equal(expected, actualVV)); + // RAW ACCESS Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696; Vector fast_y = gtsam::zero(8); double xvalues[8] = {1,2,3,4,0,0,5,6}; - factor.multiplyHessianAdd(1, xvalues, fast_y.data()); + factor.multiplyHessianAdd(alpha, xvalues, fast_y.data()); EXPECT(assert_equal(expected_y, fast_y)); // now, do it with non-zero y - factor.multiplyHessianAdd(1, xvalues, fast_y.data()); + factor.multiplyHessianAdd(alpha, xvalues, fast_y.data()); EXPECT(assert_equal(2*expected_y, fast_y)); // check some expressions