diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index 8b85b74fd..23fa219ab 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -56,8 +56,13 @@ public: JacobianFactor::multiplyHessianAdd(alpha, x, y); } + // 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, + // then accumulatedDims is [0 3 9 11 13] + // NOTE: size of accumulatedDims is size of keys + 1!! void multiplyHessianAdd(double alpha, const double* x, double* y, - std::vector keys) const { + const std::vector& accumulatedDims) const { // Use eigen magic to access raw memory typedef Eigen::Matrix DVector; @@ -68,13 +73,13 @@ public: return; Vector Ax = zero(Ab_.rows()); - // Just iterate over all A matrices and multiply in correct config part + // Just iterate over all A matrices and multiply in correct config part (looping over keys) + // E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' + // Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) for (size_t pos = 0; pos < size(); ++pos) { - std::cout << "pos: " << pos << " keys_[pos]: " << keys_[pos] << " keys[keys_[pos]]: " << keys[keys_[pos]] << std::endl; Ax += Ab_(pos) - * ConstDMap(x + keys[keys_[pos]], - keys[keys_[pos] + 1] - keys[keys_[pos]]); + * ConstDMap(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); } // Deal with noise properly, need to Double* whiten as we are dividing by variance if (model_) { @@ -85,10 +90,11 @@ public: // multiply with alpha Ax *= alpha; - // Again iterate over all A matrices and insert Ai^e into y - for (size_t pos = 0; pos < size(); ++pos) - DMap(y + keys[keys_[pos]], keys[keys_[pos] + 1] - keys[keys_[pos]]) += Ab_( + // Again iterate over all A matrices and insert Ai^T into y + for (size_t pos = 0; pos < size(); ++pos){ + DMap(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( pos).transpose() * Ax; + } } void multiplyHessianAdd(double alpha, const double* x, double* y) const { @@ -123,10 +129,9 @@ public: /// Raw memory access version of hessianDiagonal /* ************************************************************************* */ - // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n + // TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n void hessianDiagonal(double* d) const { // Use eigen magic to access raw memory - //typedef Eigen::Matrix DVector; typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; @@ -134,11 +139,15 @@ public: for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert its diagonal DVector dj; - //for (size_t k = 0; k < 9; ++k) - for (size_t k = 0; k < D; ++k) - dj(k) = Ab_(j).col(k).squaredNorm(); - - //DMap(d + 9 * j) += dj; + for (size_t k = 0; k < D; ++k){ + if (model_){ + Vector column_k = Ab_(j).col(k); + column_k = model_->whiten(column_k); + dj(k) = dot(column_k, column_k); + }else{ + dj(k) = Ab_(j).col(k).squaredNorm(); + } + } DMap(d + D * j) += dj; } } diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp index 0377fd3ee..397e5949c 100644 --- a/gtsam/slam/tests/testRegularJacobianFactor.cpp +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -22,103 +22,161 @@ using namespace std; using namespace gtsam; using namespace boost::assign; +static const size_t fixedDim = 3; +static const size_t nrKeys = 3; + +// Keys are assumed to be from 0 to n namespace { namespace simple { // Terms we'll use const vector > terms = list_of > - (make_pair(5, Matrix3::Identity())) - (make_pair(10, 2*Matrix3::Identity())) - (make_pair(15, 3*Matrix3::Identity())); + (make_pair(0, Matrix3::Identity())) + (make_pair(1, 2*Matrix3::Identity())) + (make_pair(2, 3*Matrix3::Identity())); // RHS and sigmas const Vector b = (Vector(3) << 1., 2., 3.); - const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.5)); + const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5,0.5,0.5)); } } +/* ************************************************************************* */ +// Convert from double* to VectorValues +VectorValues double2vv(const double* x, + const size_t nrKeys, const size_t dim) { + // create map with dimensions + std::map dims; + for (size_t i = 0; i < nrKeys; i++) + dims.insert(make_pair(i, dim)); + + size_t n = nrKeys*dim; + Vector xVec(n); + for (size_t i = 0; i < n; i++){ + xVec(i) = x[i]; + } + return VectorValues(xVec, dims); +} + +/* ************************************************************************* */ +void vv2double(const VectorValues& vv, double* y, + const size_t nrKeys, const size_t dim) { + // create map with dimensions + std::map dims; + for (size_t i = 0; i < nrKeys; i++) + dims.insert(make_pair(i, dim)); + + Vector yvector = vv.vector(dims); + size_t n = nrKeys*dim; + for (size_t j = 0; j < n; j++) + y[j] = yvector[j]; +} + + /* ************************************************************************* */ TEST(RegularJacobianFactor, constructorNway) { using namespace simple; - JacobianFactor expected(terms[0].first, terms[0].second, + JacobianFactor factor(terms[0].first, terms[0].second, terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); - RegularJacobianFactor<3> actual(terms, b, noise); + RegularJacobianFactor regularFactor(terms, b, noise); - LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); - EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); - EXPECT(assert_equal(b, expected.getb())); - EXPECT(assert_equal(b, actual.getb())); - EXPECT(noise == expected.get_model()); - EXPECT(noise == actual.get_model()); + LONGS_EQUAL((long)terms[2].first, (long)regularFactor.keys().back()); + EXPECT(assert_equal(terms[2].second, regularFactor.getA(regularFactor.end() - 1))); + EXPECT(assert_equal(b, factor.getb())); + EXPECT(assert_equal(b, regularFactor.getb())); + EXPECT(noise == factor.get_model()); + EXPECT(noise == regularFactor.get_model()); } TEST(RegularJacobianFactor, hessianDiagonal) { using namespace simple; - JacobianFactor expected(terms[0].first, terms[0].second, + JacobianFactor factor(terms[0].first, terms[0].second, terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); - RegularJacobianFactor<3> actual(terms, b, noise); + RegularJacobianFactor regularFactor(terms, b, noise); - EXPECT(assert_equal(expected.hessianDiagonal(),actual.hessianDiagonal())); - expected.hessianDiagonal().print(); - actual.hessianDiagonal().print(); + // 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]; - actual.hessianDiagonal(actualValue); - for(int i=0; i<9; ++i) - std::cout << actualValue[i] << std::endl; - - // Why unwhitened? + regularFactor.hessianDiagonal(actualValue); + VectorValues actualHessianDiagonalRaw = double2vv(actualValue,nrKeys,fixedDim); + EXPECT(assert_equal(expectedHessianDiagonal,actualHessianDiagonalRaw)); } +/* ************************************************************************* */ TEST(RegularJacobian, gradientAtZero) { using namespace simple; - JacobianFactor expected(terms[0].first, terms[0].second, + JacobianFactor factor(terms[0].first, terms[0].second, terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); - RegularJacobianFactor<3> actual(terms, b, noise); - EXPECT(assert_equal(expected.gradientAtZero(),actual.gradientAtZero())); + RegularJacobianFactor regularFactor(terms, b, noise); + EXPECT(assert_equal(factor.gradientAtZero(),regularFactor.gradientAtZero())); + // create and test raw memory access version // raw memory access is not available now } +/* ************************************************************************* */ TEST(RegularJacobian, multiplyHessianAdd) { using namespace simple; - RegularJacobianFactor<3> factor(terms, b, noise); + JacobianFactor factor(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); + RegularJacobianFactor regularFactor(terms, b, noise); + // arbitrary vector X VectorValues X; - X.insert(5, (Vector(3) << 10.,20.,30.)); - X.insert(10, (Vector(3) << 10.,20.,30.)); - X.insert(15, (Vector(3) << 10.,20.,30.)); + X.insert(0, (Vector(3) << 10.,20.,30.)); + X.insert(1, (Vector(3) << 10.,20.,30.)); + X.insert(2, (Vector(3) << 10.,20.,30.)); + // arbitrary vector Y VectorValues Y; - Y.insert(5, (Vector(3) << 10.,10.,10.)); - Y.insert(10, (Vector(3) << 20.,20.,20.)); - Y.insert(15, (Vector(3) << 30.,30.,30.)); + Y.insert(0, (Vector(3) << 10.,10.,10.)); + Y.insert(1, (Vector(3) << 20.,20.,20.)); + Y.insert(2, (Vector(3) << 30.,30.,30.)); - // muultiplyHessianAdd Y += alpha*A'A*X - factor.multiplyHessianAdd(2.0, X, Y); + // multiplyHessianAdd Y += alpha*A'A*X + double alpha = 2.0; + VectorValues expectedMHA = Y; + factor.multiplyHessianAdd(alpha, X, expectedMHA); - VectorValues expectedValues; - expectedValues.insert(5, (Vector(3) << 490.,970.,1450.)); - expectedValues.insert(10, (Vector(3) << 980.,1940.,2900.)); - expectedValues.insert(15, (Vector(3) << 1470.,2910.,4350.)); + VectorValues actualMHA = Y; + regularFactor.multiplyHessianAdd(alpha, X, actualMHA); - EXPECT(assert_equal(expectedValues,Y)); + EXPECT(assert_equal(expectedMHA, actualMHA)); - //double dataX[9] = {10., 20., 30., 10., 20., 30., 10., 20., 30.}; - //double dataY[9] = {10., 10., 10., 20., 20., 20., 30., 30., 30.}; + // create data for raw memory access + double XRaw[9]; + vv2double(X, XRaw, nrKeys, fixedDim); - std::cout << "size: " << factor.size() << std::endl; - double dataX[9] = {0.,}; - double dataY[9] = {0.,}; - - std::vector ks; - ks.push_back(5);ks.push_back(10);ks.push_back(15); - - // Raw memory access version - factor.multiplyHessianAdd(2.0, dataX, dataY, ks); + // test 1st version: multiplyHessianAdd(double alpha, const double* x, double* y) + double actualMHARaw[9]; + vv2double(Y, actualMHARaw, nrKeys, fixedDim); + regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw); + VectorValues actualMHARawVV = double2vv(actualMHARaw,nrKeys,fixedDim); + EXPECT(assert_equal(expectedMHA,actualMHARawVV)); + // test 2nd version: multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) + double actualMHARaw2[9]; + vv2double(Y, actualMHARaw2, nrKeys, fixedDim); + vector dims; + size_t accumulatedDim = 0; + for (size_t i = 0; i < nrKeys+1; i++){ + dims.push_back(accumulatedDim); + accumulatedDim += fixedDim; + } + regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw2, dims); + VectorValues actualMHARawVV2 = double2vv(actualMHARaw2,nrKeys,fixedDim); + EXPECT(assert_equal(expectedMHA,actualMHARawVV2)); } /* ************************************************************************* */