diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 58eaf4460..5fb2e15bd 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -118,6 +118,12 @@ namespace gtsam { /// y += alpha * A'*A*x virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0; + /// y += alpha * A'*A*x + virtual void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const = 0; + + /// y += alpha * A'*A*x + virtual void multiplyHessianAdd(double alpha, const double* x, double* y) const = 0; + /// A'*b for Jacobian, eta for Hessian virtual VectorValues gradientAtZero() const = 0; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 873838918..b56270a55 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -54,6 +54,27 @@ namespace gtsam { return keys; } + /* ************************************************************************* */ + vector GaussianFactorGraph::getkeydim() const { + // First find dimensions of each variable + vector dims; + BOOST_FOREACH(const sharedFactor& factor, *this) { + for (GaussianFactor::const_iterator pos = factor->begin(); + pos != factor->end(); ++pos) { + if (dims.size() <= *pos) + dims.resize(*pos + 1, 0); + dims[*pos] = factor->getDim(pos); + } + } + // Find accumulated dimensions for variables + vector dims_accumulated; + dims_accumulated.resize(dims.size()+1,0); + dims_accumulated[0]=0; + for (int i=1; imultiplyHessianAdd(alpha, x, y); + f->multiplyHessianAdd(alpha, x, y); + } + + /* ************************************************************************* */ + void GaussianFactorGraph::multiplyHessianAdd(double alpha, + const double* x, double* y) const { + vector FactorKeys = getkeydim(); + BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this) + f->multiplyHessianAdd(alpha, x, y, FactorKeys); + } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 28b9eab55..267792f33 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -135,6 +135,8 @@ namespace gtsam { typedef FastSet Keys; Keys keys() const; + std::vector getkeydim() const; + /** unnormalized error */ double error(const VectorValues& x) const { double total_error = 0.; @@ -296,6 +298,10 @@ namespace gtsam { void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; + ///** y += alpha*A'A*x */ + void multiplyHessianAdd(double alpha, const double* x, + double* y) const; + ///** In-place version e <- A*x that overwrites e. */ void multiplyInPlace(const VectorValues& x, Errors& e) const; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index fced268ca..aa1e4e6eb 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -525,7 +525,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, // copy to yvalues for(DenseIndex i = 0; i < (DenseIndex)size(); ++i) { - bool didNotExist; + bool didNotExist; VectorValues::iterator it; boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector()); if (didNotExist) @@ -535,6 +535,38 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, } } +/* ************************************************************************* */ +void HessianFactor::multiplyHessianAdd(double alpha, const double* x, + double* yvalues, vector keys) const { + + // 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+keys[keys_[j]],keys[keys_[j]+1]-keys[keys_[j]]); + // blocks on the diagonal are only half + y[i] += info_(j, j).selfadjointView() * ConstDMap(x+keys[keys_[j]],keys[keys_[j]+1]-keys[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+keys[keys_[j]],keys[keys_[j]+1]-keys[keys_[j]]); + } + + // copy to yvalues + for(DenseIndex i = 0; i < (DenseIndex)size(); ++i) + DMap(yvalues+keys[keys_[i]],keys[keys_[i]+1]-keys[keys_[i]]) += alpha * y[i]; + + +} + + /* ************************************************************************* */ VectorValues HessianFactor::gradientAtZero() const { VectorValues g; diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 18c238e57..91e4c8a08 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -141,6 +141,12 @@ namespace gtsam { typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix typedef SymmetricBlockMatrix::constBlock constBlock; ///< A block from the Hessian matrix (const version) + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + /** default constructor for I/O */ HessianFactor(); @@ -376,6 +382,10 @@ 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; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index a374a37d4..e62f8e9ef 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -495,6 +495,7 @@ namespace gtsam { if(xi.second) xi.first->second = Vector::Zero(getDim(begin() + pos)); gtsam::transposeMultiplyAdd(Ab_(pos), E, xi.first->second); + } } @@ -505,6 +506,26 @@ namespace gtsam { transposeMultiplyAdd(alpha,Ax,y); } + void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const { + if (empty()) return; + Vector Ax = zero(Ab_.rows()); + + // Just iterate over all A matrices and multiply in correct config part + for(size_t pos=0; poswhitenInPlace(Ax); model_->whitenInPlace(Ax); } + + // multiply with alpha + Ax *= alpha; + + // Again iterate over all A matrices and insert Ai^e into y + for(size_t pos=0; pos DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + + /** Convert from other GaussianFactor */ explicit JacobianFactor(const GaussianFactor& gf); @@ -275,6 +281,10 @@ 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 {}; + /// A'*b for Jacobian VectorValues gradientAtZero() const; diff --git a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp index 60fca2d8f..13038bcc2 100644 --- a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp @@ -166,9 +166,9 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), (Vector(2) << 2.0, -1.0), unit2); + fg += JacobianFactor(0, 10*eye(2), 2, -10*eye(2), (Vector(2) << 2.0, -1.0), unit2); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), (Vector(2) << 0.0, 1.0), unit2); + fg += JacobianFactor(1, 5*eye(2), 2, -5*eye(2), (Vector(2) << 0.0, 1.0), unit2); // measurement between x2 and l1: l1-x2=[-0.2;0.3] fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5), unit2); return fg; @@ -313,6 +313,31 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 ) EXPECT(assert_equal(2*expected, actual)); } +/* ************************************************************************* */ +TEST( GaussianFactorGraph, multiplyHessianAdd3 ) +{ + GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); + + // brute force + Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); + Vector X(6); X<<1,2,3,4,5,6; + Vector Y(6); Y<<-450, -450, 300, 400, 2950, 3450; + EXPECT(assert_equal(Y,AtA*X)); + + double* x = &X[0]; + double* y = &Y[0]; + + Vector fast_y = gtsam::zero(6); + double* actual = &fast_y[0]; + gfg.multiplyHessianAdd(1.0, x, fast_y.data()); + EXPECT(assert_equal(Y, fast_y)); + + // now, do it with non-zero y + gfg.multiplyHessianAdd(1.0, x, fast_y.data()); + EXPECT(assert_equal(2*Y, fast_y)); + +} + /* ************************************************************************* */ TEST( GaussianFactorGraph, matricesMixed )