From 51df17ffdf559453f69ed758e23872612dacf102 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 12 Nov 2014 04:17:09 -0500 Subject: [PATCH 01/32] Remove virtual functions using raw memory access --- gtsam/linear/GaussianFactor.h | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 8c8e2cb2b..58eaf4460 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -99,9 +99,6 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const = 0; - /// Return the diagonal of the Hessian for this factor (raw memory version) - virtual void hessianDiagonal(double* d) const = 0; - /// Return the block diagonal of the Hessian for this factor virtual std::map hessianBlockDiagonal() const = 0; @@ -121,18 +118,9 @@ 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; - /// A'*b for Jacobian, eta for Hessian (raw memory version) - virtual void gradientAtZero(double* d) const = 0; - private: /** Serialization function */ friend class boost::serialization::access; From a143815e79e9cbdbbb0298048450fdca3601f96f Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 12 Nov 2014 04:18:49 -0500 Subject: [PATCH 02/32] Comment out raw memory access parts --- gtsam/linear/GaussianFactorGraph.cpp | 15 +++++++-------- gtsam/linear/GaussianFactorGraph.h | 4 ++-- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 54e721cd7..ad43b675b 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -355,14 +355,13 @@ namespace gtsam { 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); - - } + ///* ************************************************************************* */ + //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); + //} /* ************************************************************************* */ void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const { diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 910b25d1e..1861e2607 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -311,8 +311,8 @@ namespace gtsam { VectorValues& y) const; ///** y += alpha*A'A*x */ - void multiplyHessianAdd(double alpha, const double* x, - double* y) const; + //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; From 102974588a4baf9c249b78b365b3f100e98bb4e7 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 12 Nov 2014 04:19:44 -0500 Subject: [PATCH 03/32] Comment out the test for raw memory access --- .../linear/tests/testGaussianFactorGraph.cpp | 33 ++++++++++--------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index d789c42fd..2fc1e359b 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -316,27 +316,28 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 ) } /* ************************************************************************* */ -TEST( GaussianFactorGraph, multiplyHessianAdd3 ) -{ - GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); +// Raw memory access +//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)); +// // 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* x = &X[0]; - Vector fast_y = gtsam::zero(6); - gfg.multiplyHessianAdd(1.0, x, fast_y.data()); - EXPECT(assert_equal(Y, fast_y)); +// Vector fast_y = gtsam::zero(6); +// 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)); +// // now, do it with non-zero y +// gfg.multiplyHessianAdd(1.0, x, fast_y.data()); +// EXPECT(assert_equal(2*Y, fast_y)); -} +//} /* ************************************************************************* */ From 84f60184812dd54bccd661b4963f7d85bc0f13dc Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 12 Nov 2014 04:21:06 -0500 Subject: [PATCH 04/32] Remove raw memory access codes --- gtsam/linear/HessianFactor.cpp | 76 ---------------------------------- gtsam/linear/HessianFactor.h | 9 ---- 2 files changed, 85 deletions(-) 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 From fe7fc8a6ef532dc646baf82cb25558301f8446a0 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 12 Nov 2014 04:21:46 -0500 Subject: [PATCH 05/32] Remove raw memory access codes --- gtsam/linear/JacobianFactor.cpp | 58 --------------------------------- gtsam/linear/JacobianFactor.h | 10 ------ 2 files changed, 68 deletions(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index a63bbf473..723d84d57 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -460,25 +460,6 @@ VectorValues JacobianFactor::hessianDiagonal() const { return d; } -/* ************************************************************************* */ -// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n -void JacobianFactor::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 j = 0; j < (DenseIndex) size(); ++j) { - // Get the diagonal block, and insert its diagonal - DVector dj; - for (size_t k = 0; k < 9; ++k) - dj(k) = Ab_(j).col(k).squaredNorm(); - - DMap(d + 9 * j) += dj; - } -} - /* ************************************************************************* */ map JacobianFactor::hessianBlockDiagonal() const { map blocks; @@ -528,40 +509,6 @@ void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, transposeMultiplyAdd(alpha, Ax, y); } -void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, - double* y, std::vector keys) const { - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - - 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; pos < size(); ++pos) - Ax += Ab_(pos) - * ConstDMap(x + keys[keys_[pos]], - keys[keys_[pos] + 1] - keys[keys_[pos]]); - - // Deal with noise properly, need to Double* whiten as we are dividing by variance - if (model_) { - model_->whitenInPlace(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 < size(); ++pos) - DMap(y + keys[keys_[pos]], keys[keys_[pos] + 1] - keys[keys_[pos]]) += Ab_( - pos).transpose() * Ax; - -} - /* ************************************************************************* */ VectorValues JacobianFactor::gradientAtZero() const { VectorValues g; @@ -573,11 +520,6 @@ VectorValues JacobianFactor::gradientAtZero() const { return g; } -/* ************************************************************************* */ -void JacobianFactor::gradientAtZero(double* d) const { - //throw std::runtime_error("gradientAtZero not implemented for Jacobian factor"); -} - /* ************************************************************************* */ pair JacobianFactor::jacobian() const { pair result = jacobianUnweighted(); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 99ea91cd9..6057b7819 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -185,9 +185,6 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const; - /// Raw memory access version of hessianDiagonal - virtual void hessianDiagonal(double* d) const; - /// Return the block diagonal of the Hessian for this factor virtual std::map hessianBlockDiagonal() const; @@ -279,16 +276,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 {}; - /// A'*b for Jacobian VectorValues gradientAtZero() const; - /* ************************************************************************* */ - virtual void gradientAtZero(double* d) const; - /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ JacobianFactor whiten() const; From fe774981162c04882b183f2bf41992ec6e365025 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 12 Nov 2014 04:23:33 -0500 Subject: [PATCH 06/32] Modify and add raw memory access for HessianFactor --- gtsam/slam/RegularHessianFactor.h | 90 ++++++++++++++++++++++++++----- 1 file changed, 77 insertions(+), 13 deletions(-) diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index 0e20bb709..995fd1f04 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -31,6 +31,9 @@ private: typedef Eigen::Matrix MatrixDD; // camera hessian block typedef Eigen::Matrix VectorD; + // Use eigen magic to access raw memory + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; public: @@ -51,30 +54,75 @@ public: HessianFactor(keys, augmentedInformation) { } + /** 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 { + // 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 + D * j) += B.diagonal(); + } + } + /** 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; - - void multiplyHessianAdd(double alpha, const double* x, - double* yvalues) const { + void multiplyHessianAdd(double alpha, const double* x, double* yvalues, + std::vector offsets) const { // Create a vector of temporary y values, corresponding to rows i - y.resize(size()); - BOOST_FOREACH(DVector & yi, y) - yi.setZero(); - - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; + std::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 - DVector xj(D); + 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]; + } + + // Scratch space for multiplyHessianAdd + mutable std::vector y; + + 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(VectorD & yi, y) + yi.setZero(); + + // 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 + VectorD xj(D); for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { Key key = keys_[j]; const double* xj = x + key * D; @@ -95,6 +143,22 @@ public: } } + /** eta for Hessian */ + VectorValues gradientAtZero() const { + return HessianFactor::gradientAtZero(); + } + + /** eta for Hessian (raw memory version) */ + void gradientAtZero(double* d) const { + // 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 + VectorD dj = -info_(pos,size()).knownOffDiagonal(); + DMap(d + D * j) += dj; + } + } + }; } From 2acb5a261149de788ba88bd4ce664b129e86ec33 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 12 Nov 2014 04:25:05 -0500 Subject: [PATCH 07/32] Create JacobianFactor derived class for fixed size and add raw memory access --- gtsam/slam/RegularJacobianFactor.h | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 gtsam/slam/RegularJacobianFactor.h diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h new file mode 100644 index 000000000..0223c66ab --- /dev/null +++ b/gtsam/slam/RegularJacobianFactor.h @@ -0,0 +1,4 @@ +#ifndef REGULARJACOBIANFACTOR_H +#define REGULARJACOBIANFACTOR_H + +#endif // REGULARJACOBIANFACTOR_H From 7a504f3babdc05684cc97b6605788e6524353f8a Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Wed, 12 Nov 2014 04:25:28 -0500 Subject: [PATCH 08/32] Create JacobianFactor derived class for fixed size and add raw memory access --- gtsam/slam/RegularJacobianFactor.h | 143 ++++++++++++++++++++++++++++- 1 file changed, 140 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index 0223c66ab..a518505ca 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -1,4 +1,141 @@ -#ifndef REGULARJACOBIANFACTOR_H -#define REGULARJACOBIANFACTOR_H +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file RegularJacobianFactor.h + * @brief JacobianFactor class with fixed sized blcoks + * @author Sungtae An + * @date Nov 11, 2014 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +template +class RegularJacobianFactor: public JacobianFactor { + +private: + + typedef Eigen::Matrix MatrixDD; // camera hessian block + typedef Eigen::Matrix VectorD; + // Use eigen magic to access raw memory + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + +public: + + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. */ + template + RegularJacobianFactor(const TERMS& terms, const Vector& b, + const SharedDiagonal& model = SharedDiagonal()) : + JacobianFactor(terms, b, model) { + } + + /** Constructor with arbitrary number keys, and where the augmented matrix is given all together + * instead of in block terms. Note that only the active view of the provided augmented matrix + * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed + * factor. */ + template + RegularJacobianFactor(const KEYS& keys, + const VerticalBlockMatrix& augmentedMatrix, + const SharedDiagonal& sigmas = SharedDiagonal()) : + JacobianFactor(keys, augmentedMatrix, sigmas) { + } + + /// Return the diagonal of the Hessian for this factor + VectorValues hessianDiagonal() const { + return JacobianFactor::hessianDiagonal(); + } + + /// Raw memory access version of hessianDiagonal + 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 + DVector dj; + for (size_t k = 0; k < D; ++k) + dj(k) = Ab_(j).col(k).squaredNorm(); + + DMap(d + D * j) += dj; + } + } + + /// y += alpha * A'*A*x + void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + JacobianFactor::multiplyHessianAdd(alpha, x, y); + } + + void 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; pos < size(); ++pos) + Ax += Ab_(pos) + * ConstDMap(x + keys[keys_[pos]], + keys[keys_[pos] + 1] - keys[keys_[pos]]); + + // Deal with noise properly, need to Double* whiten as we are dividing by variance + if (model_) { + model_->whitenInPlace(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 < size(); ++pos) + DMap(y + keys[keys_[pos]], keys[keys_[pos] + 1] - keys[keys_[pos]]) += Ab_( + pos).transpose() * Ax; + } + + void multiplyHessianAdd(double alpha, const double* x, double* y) 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 Date: Fri, 14 Nov 2014 14:07:37 -0500 Subject: [PATCH 09/32] Unit test for Regular Hessian Factor --- gtsam/slam/tests/testRegularHessianFactor.cpp | 109 ++++++++++++++++++ 1 file changed, 109 insertions(+) create mode 100644 gtsam/slam/tests/testRegularHessianFactor.cpp diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp new file mode 100644 index 000000000..14de545a6 --- /dev/null +++ b/gtsam/slam/tests/testRegularHessianFactor.cpp @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testRegularHessianFactor.cpp + * @author Frank Dellaert + * @date March 4, 2014 + */ + +#include +#include + +//#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +const double tol = 1e-5; + +/* ************************************************************************* */ +TEST(RegularHessianFactor, ConstructorNWay) +{ + Matrix G11 = (Matrix(2,2) << 111, 112, 113, 114); + Matrix G12 = (Matrix(2,2) << 121, 122, 123, 124); + Matrix G13 = (Matrix(2,2) << 131, 132, 133, 134); + + Matrix G22 = (Matrix(2,2) << 221, 222, 222, 224); + Matrix G23 = (Matrix(2,2) << 231, 232, 233, 234); + + Matrix G33 = (Matrix(2,2) << 331, 332, 332, 334); + + Vector g1 = (Vector(2) << -7, -9); + Vector g2 = (Vector(2) << -9, 1); + Vector g3 = (Vector(2) << 2, 3); + + double f = 10; + + std::vector js; + js.push_back(0); js.push_back(1); js.push_back(3); + std::vector Gs; + Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33); + std::vector gs; + gs.push_back(g1); gs.push_back(g2); gs.push_back(g3); + RegularHessianFactor<2> factor(js, Gs, gs, f); + + // multiplyHessianAdd: + { + // brute force + Matrix AtA = factor.information(); + HessianFactor::const_iterator i1 = factor.begin(); + HessianFactor::const_iterator i2 = i1 + 1; + Vector X(6); X << 1,2,3,4,5,6; + Vector Y(6); Y << 2633, 2674, 4465, 4501, 5669, 5696; + EXPECT(assert_equal(Y,AtA*X)); + + VectorValues x = map_list_of + (0, (Vector(2) << 1,2)) + (1, (Vector(2) << 3,4)) + (3, (Vector(2) << 5,6)); + + VectorValues expected; + expected.insert(0, Y.segment<2>(0)); + 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); + double xvalues[8] = {1,2,3,4,0,0,5,6}; + factor.multiplyHessianAdd(1, 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()); + EXPECT(assert_equal(2*expected_y, fast_y)); + + // check some expressions + EXPECT(assert_equal(G12,factor.info(i1,i2).knownOffDiagonal())); + EXPECT(assert_equal(G22,factor.info(i2,i2).selfadjointView())); + EXPECT(assert_equal((Matrix)G12.transpose(),factor.info(i2,i1).knownOffDiagonal())); + } +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ From 95adc49ac392699b6e8629b569c51d202ab9d97d Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 14 Nov 2014 14:07:53 -0500 Subject: [PATCH 10/32] Unit test for Regular Jacobian Factor --- .../slam/tests/testRegularJacobianFactor.cpp | 126 ++++++++++++++++++ 1 file changed, 126 insertions(+) create mode 100644 gtsam/slam/tests/testRegularJacobianFactor.cpp diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp new file mode 100644 index 000000000..0377fd3ee --- /dev/null +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -0,0 +1,126 @@ +/** + * @file testRegularJacobianFactor.cpp + * @brief unit test regular jacobian factors + * @author Sungtae An + * @date Nov 12, 2014 + */ + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +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())); + + // 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)); + } +} + +/* ************************************************************************* */ +TEST(RegularJacobianFactor, constructorNway) +{ + using namespace simple; + JacobianFactor expected(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); + + 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()); +} + +TEST(RegularJacobianFactor, hessianDiagonal) +{ + using namespace simple; + JacobianFactor expected(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.hessianDiagonal(),actual.hessianDiagonal())); + expected.hessianDiagonal().print(); + actual.hessianDiagonal().print(); + double actualValue[9]; + actual.hessianDiagonal(actualValue); + for(int i=0; i<9; ++i) + std::cout << actualValue[i] << std::endl; + + // Why unwhitened? +} + +TEST(RegularJacobian, gradientAtZero) +{ + using namespace simple; + JacobianFactor expected(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())); + + // raw memory access is not available now +} + +TEST(RegularJacobian, multiplyHessianAdd) +{ + using namespace simple; + RegularJacobianFactor<3> factor(terms, b, noise); + + 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.)); + + 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.)); + + // muultiplyHessianAdd Y += alpha*A'A*X + factor.multiplyHessianAdd(2.0, X, Y); + + 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.)); + + EXPECT(assert_equal(expectedValues,Y)); + + //double dataX[9] = {10., 20., 30., 10., 20., 30., 10., 20., 30.}; + //double dataY[9] = {10., 10., 10., 20., 20., 20., 30., 30., 30.}; + + 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); + +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ From 2733b66a2324ca238ba5b837b790255d54c00c57 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 14 Nov 2014 14:08:17 -0500 Subject: [PATCH 11/32] Add a few comments --- gtsam/linear/Preconditioner.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 9af362fba..c7f4a5b68 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -125,7 +125,9 @@ void BlockJacobiPreconditioner::transposeSolve(const Vector& y, Vector& x) const void BlockJacobiPreconditioner::build( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda) { + // n is the number of keys const size_t n = keyInfo.size(); + // dims_ is a vector that contains the dimension of keys dims_ = keyInfo.colSpec(); /* prepare the buffer of block diagonals */ From ee3c7ce1826412ff1773a4c1d494658cac61e4a1 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 14 Nov 2014 14:09:42 -0500 Subject: [PATCH 12/32] Add a different test for building blocks --- tests/testPreconditioner.cpp | 41 ++++++++++++++++++++++++++++++++---- 1 file changed, 37 insertions(+), 4 deletions(-) diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 40d49a934..46b243913 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -42,6 +42,21 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { return fg; } +/* ************************************************************************* */ +static GaussianFactorGraph createSimpleGaussianFactorGraphUnordered() { + GaussianFactorGraph fg; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + // 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), 1, 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); + // 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; +} + /* ************************************************************************* */ // Copy of BlockJacobiPreconditioner::build std::vector buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo) @@ -89,10 +104,6 @@ std::vector buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo & TEST( Preconditioner, buildBlocks ) { // Create simple Gaussian factor graph and initial values GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); - Values initial; - initial.insert(0,Point2(4, 5)); - initial.insert(1,Point2(0, 1)); - initial.insert(2,Point2(-5, 7)); // Expected Hessian block diagonal matrices std::map expectedHessian =gfg.hessianBlockDiagonal(); @@ -110,6 +121,28 @@ TEST( Preconditioner, buildBlocks ) { EXPECT(assert_equal(it1->second, *it2)); } +/* ************************************************************************* */ +TEST( Preconditioner, buildBlocks2 ) { + // Create simple Gaussian factor graph and initial values + GaussianFactorGraph gfg = createSimpleGaussianFactorGraphUnordered(); + + // Expected Hessian block diagonal matrices + std::map expectedHessian =gfg.hessianBlockDiagonal(); + + // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build + std::vector actualHessian = buildBlocks(gfg, KeyInfo(gfg)); + + // Compare the number of block diagonal matrices + EXPECT_LONGS_EQUAL(expectedHessian.size(), 3); + EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); + + // Compare the values of matrices + std::map::const_iterator it1 = expectedHessian.begin(); + std::vector::const_iterator it2 = actualHessian.begin(); + for(; it1!=expectedHessian.end(); it1++, it2++) + EXPECT(assert_equal(it1->second, *it2)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 03ebcb618545afb61e7a8e0462322673c03ecd31 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 14 Nov 2014 14:10:08 -0500 Subject: [PATCH 13/32] Add some changes --- gtsam/slam/RegularHessianFactor.h | 117 +++++++++++++++++------------ gtsam/slam/RegularJacobianFactor.h | 70 ++++++++++------- 2 files changed, 110 insertions(+), 77 deletions(-) diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index 995fd1f04..92e7d92bc 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -31,9 +31,6 @@ private: typedef Eigen::Matrix MatrixDD; // camera hessian block typedef Eigen::Matrix VectorD; - // Use eigen magic to access raw memory - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; public: @@ -54,30 +51,57 @@ public: HessianFactor(keys, augmentedInformation) { } - /** 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 { - // 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 + D * j) += B.diagonal(); - } - } - /** 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; + + 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) + yi.setZero(); + + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + + // 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 + DVector xj(D); + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + Key key = keys_[j]; + const double* xj = x + key * D; + DenseIndex i = 0; + for (; i < j; ++i) + y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); + // blocks on the diagonal are only half + y[i] += info_(j, j).selfadjointView() * ConstDMap(xj); + // 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(xj); + } + + // copy to yvalues + for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { + Key key = keys_[i]; + DMap(yvalues + key * D) += alpha * y[i]; + } + } + void multiplyHessianAdd(double alpha, const double* x, double* yvalues, std::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 std::vector y; y.reserve(size()); @@ -110,52 +134,45 @@ public: alpha * y[i]; } - // Scratch space for multiplyHessianAdd - mutable std::vector y; + /** Return the diagonal of the Hessian for this factor */ + VectorValues hessianDiagonal() const { + return HessianFactor::hessianDiagonal(); + } - 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(VectorD & yi, y) - yi.setZero(); + /** Return the diagonal of the Hessian for this factor (raw memory version) */ + void hessianDiagonal(double* d) const { - // 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 - VectorD xj(D); - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - Key key = keys_[j]; - const double* xj = x + key * D; - DenseIndex i = 0; - for (; i < j; ++i) - y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); - // blocks on the diagonal are only half - y[i] += info_(j, j).selfadjointView() * ConstDMap(xj); - // 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(xj); - } + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; - // copy to yvalues - for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { - Key key = keys_[i]; - DMap(yvalues + key * D) += alpha * y[i]; + // 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(); } } - /** eta for Hessian */ VectorValues gradientAtZero() const { return HessianFactor::gradientAtZero(); } - /** eta for Hessian (raw memory version) */ + /* ************************************************************************* */ + // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n void gradientAtZero(double* d) const { - // Loop over all variables in the factor + + // 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 VectorD dj = -info_(pos,size()).knownOffDiagonal(); - DMap(d + D * j) += dj; + DMap(d + 9 * j) += dj; } } diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index a518505ca..8b85b74fd 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include #include @@ -27,14 +28,6 @@ namespace gtsam { template class RegularJacobianFactor: public JacobianFactor { -private: - - typedef Eigen::Matrix MatrixDD; // camera hessian block - typedef Eigen::Matrix VectorD; - // Use eigen magic to access raw memory - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - public: /** Construct an n-ary factor @@ -57,24 +50,6 @@ public: JacobianFactor(keys, augmentedMatrix, sigmas) { } - /// Return the diagonal of the Hessian for this factor - VectorValues hessianDiagonal() const { - return JacobianFactor::hessianDiagonal(); - } - - /// Raw memory access version of hessianDiagonal - 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 - DVector dj; - for (size_t k = 0; k < D; ++k) - dj(k) = Ab_(j).col(k).squaredNorm(); - - DMap(d + D * j) += dj; - } - } - /// y += alpha * A'*A*x void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const { @@ -83,16 +58,24 @@ public: void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + 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; 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]]); - + } // Deal with noise properly, need to Double* whiten as we are dividing by variance if (model_) { model_->whitenInPlace(Ax); @@ -109,6 +92,12 @@ public: } void multiplyHessianAdd(double alpha, const double* x, double* y) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + if (empty()) return; Vector Ax = zero(Ab_.rows()); @@ -127,6 +116,33 @@ 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 9 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; + + // Loop over all variables in the factor + 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; + DMap(d + D * j) += dj; + } + } + VectorValues gradientAtZero() const { return JacobianFactor::gradientAtZero(); } From c6faa784e2cc9a65a7ccfc606445638b850b6a3a Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 14 Nov 2014 17:41:15 -0500 Subject: [PATCH 14/32] fixed unit test with Luca --- gtsam/slam/RegularJacobianFactor.h | 39 +++-- .../slam/tests/testRegularJacobianFactor.cpp | 158 ++++++++++++------ 2 files changed, 132 insertions(+), 65 deletions(-) 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)); } /* ************************************************************************* */ From 174f60762a5e61f1cef9b99c704ec00e7d5c6cb2 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 14 Nov 2014 23:23:16 -0500 Subject: [PATCH 15/32] Add gradientAtZero (Raw memory access) --- gtsam/slam/RegularJacobianFactor.h | 87 ++++++++++++++++++------------ 1 file changed, 52 insertions(+), 35 deletions(-) diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index 23fa219ab..51a4a27a1 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -28,6 +28,13 @@ namespace gtsam { template class RegularJacobianFactor: public JacobianFactor { +private: + + /** Use eigen magic to access raw memory */ + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + public: /** Construct an n-ary factor @@ -50,60 +57,57 @@ public: JacobianFactor(keys, augmentedMatrix, sigmas) { } - /// y += alpha * A'*A*x + /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const { 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!! + /** Raw memory access version of multiplyHessianAdd + * 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, const std::vector& accumulatedDims) const { - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; + /// Use eigen magic to access raw memory + typedef Eigen::Matrix VectorD; + typedef Eigen::Map MapD; + typedef Eigen::Map ConstMapD; if (empty()) return; Vector Ax = zero(Ab_.rows()); - // 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) + /// 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) { Ax += Ab_(pos) - * ConstDMap(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); + * ConstMapD(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 + /// Deal with noise properly, need to Double* whiten as we are dividing by variance if (model_) { model_->whitenInPlace(Ax); model_->whitenInPlace(Ax); } - // multiply with alpha + /// multiply with alpha Ax *= alpha; - // Again iterate over all A matrices and insert Ai^T into y + /// 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_( + MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( pos).transpose() * Ax; } } + /** Raw memory access version of multiplyHessianAdd */ void multiplyHessianAdd(double alpha, const double* x, double* y) const { - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - if (empty()) return; Vector Ax = zero(Ab_.rows()); @@ -122,19 +126,16 @@ 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(); - } +// /// 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 + /** 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 { - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - // Loop over all variables in the factor for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert its diagonal @@ -152,12 +153,28 @@ 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 { - //throw std::runtime_error("gradientAtZero not implemented for Jacobian factor"); + // 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 + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + // Get the diagonal block, and insert its diagonal + DVector dj; + for (size_t k = 0; k < D; ++k){ + Vector column_k = Ab_(j).col(k); + dj(k) = -1.0*dot(b_sigma,column_k); + } + DMap(d + D * j) += dj; + } } }; From 9f1730809be7e516a658450e38058693bb8cebb7 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 14 Nov 2014 23:23:55 -0500 Subject: [PATCH 16/32] Add unit test for gradientAtZero --- .../slam/tests/testRegularJacobianFactor.cpp | 26 ++++++++++++++----- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp index 397e5949c..efa603aca 100644 --- a/gtsam/slam/tests/testRegularJacobianFactor.cpp +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -71,7 +71,6 @@ void vv2double(const VectorValues& vv, double* y, y[j] = yvector[j]; } - /* ************************************************************************* */ TEST(RegularJacobianFactor, constructorNway) { @@ -88,6 +87,7 @@ TEST(RegularJacobianFactor, constructorNway) EXPECT(noise == regularFactor.get_model()); } +/* ************************************************************************* */ TEST(RegularJacobianFactor, hessianDiagonal) { using namespace simple; @@ -100,15 +100,15 @@ TEST(RegularJacobianFactor, hessianDiagonal) // we compute hessian diagonal from the regular Jacobian, but still using the // implementation of hessianDiagonal in the base class - VectorValues actualHessianDiagonal = regularFactor.hessianDiagonal(); + //VectorValues actualHessianDiagonal = regularFactor.hessianDiagonal(); - EXPECT(assert_equal(expectedHessianDiagonal,actualHessianDiagonal)); + //EXPECT(assert_equal(expectedHessianDiagonal,actualHessianDiagonal)); // we compare against the Raw memory access implementation of hessianDiagonal double actualValue[9]; regularFactor.hessianDiagonal(actualValue); VectorValues actualHessianDiagonalRaw = double2vv(actualValue,nrKeys,fixedDim); - EXPECT(assert_equal(expectedHessianDiagonal,actualHessianDiagonalRaw)); + EXPECT(assert_equal(expectedHessianDiagonal, actualHessianDiagonalRaw)); } /* ************************************************************************* */ @@ -118,10 +118,22 @@ TEST(RegularJacobian, gradientAtZero) 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); - EXPECT(assert_equal(factor.gradientAtZero(),regularFactor.gradientAtZero())); - // create and test raw memory access version - // raw memory access is not available now + // 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())); + + // 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)); + } /* ************************************************************************* */ From 960d10582d2d96235af32db2c6e8b1234151f036 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 17 Nov 2014 16:17:11 -0500 Subject: [PATCH 17/32] Add pure virtual function in GaussianFactor and empty dummy virtual function in Jacobian/Hessian Factor for the raw memory access functions --- gtsam/linear/GaussianFactor.h | 6 ++++++ gtsam/linear/HessianFactor.cpp | 14 ++++++++++++++ gtsam/linear/HessianFactor.h | 6 ++++++ gtsam/linear/JacobianFactor.cpp | 14 ++++++++++++++ gtsam/linear/JacobianFactor.h | 6 ++++++ 5 files changed, 46 insertions(+) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 58eaf4460..2b5777e94 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -99,6 +99,9 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const = 0; + /// Raw memory access version of hessianDiagonal + virtual void hessianDiagonal(double* d) const = 0; + /// Return the block diagonal of the Hessian for this factor virtual std::map hessianBlockDiagonal() const = 0; @@ -121,6 +124,9 @@ namespace gtsam { /// A'*b for Jacobian, eta for Hessian virtual VectorValues gradientAtZero() const = 0; + /// Raw memory access version of gradientAtZero + virtual void gradientAtZero(double* d) const = 0; + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index b1f8dc6a6..e42e4a1f7 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -358,6 +358,13 @@ VectorValues HessianFactor::hessianDiagonal() const { return d; } +/* ************************************************************************* */ +// Raw memory access version should be called in Regular Factors only currently +void HessianFactor::hessianDiagonal(double* d) const { + throw std::runtime_error( + "HessianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only"); +} + /* ************************************************************************* */ map HessianFactor::hessianBlockDiagonal() const { map blocks; @@ -540,6 +547,13 @@ VectorValues HessianFactor::gradientAtZero() const { return g; } +/* ************************************************************************* */ +// Raw memory access version should be called in Regular Factors only currently +void HessianFactor::gradientAtZero(double* d) const { + throw std::runtime_error( + "HessianFactor::gradientAtZero raw memory access is allowed for Regular Factors only"); +} + /* ************************************************************************* */ 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 44118cece..bcc6f4864 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -340,6 +340,9 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const; + /// Raw memory access version of hessianDiagonal + virtual void hessianDiagonal(double* d) const; + /// Return the block diagonal of the Hessian for this factor virtual std::map hessianBlockDiagonal() const; @@ -380,6 +383,9 @@ namespace gtsam { /// eta for Hessian VectorValues gradientAtZero() const; + /// Raw memory access version of gradientAtZero + 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 diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 723d84d57..4e75c5bd6 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -460,6 +460,13 @@ VectorValues JacobianFactor::hessianDiagonal() const { return d; } +/* ************************************************************************* */ +// Raw memory access version should be called in Regular Factors only currently +void JacobianFactor::hessianDiagonal(double* d) const { + throw std::runtime_error( + "JacobianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only"); +} + /* ************************************************************************* */ map JacobianFactor::hessianBlockDiagonal() const { map blocks; @@ -520,6 +527,13 @@ VectorValues JacobianFactor::gradientAtZero() const { return g; } +/* ************************************************************************* */ +// Raw memory access version should be called in Regular Factors only currently +void JacobianFactor::gradientAtZero(double* d) const { + throw std::runtime_error( + "JacobianFactor::gradientAtZero raw memory access is allowed for Regular Factors only"); +} + /* ************************************************************************* */ pair JacobianFactor::jacobian() const { pair result = jacobianUnweighted(); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 6057b7819..32f062928 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -185,6 +185,9 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const; + /// Raw memory access version of hessianDiagonal + virtual void hessianDiagonal(double* d) const; + /// Return the block diagonal of the Hessian for this factor virtual std::map hessianBlockDiagonal() const; @@ -279,6 +282,9 @@ namespace gtsam { /// A'*b for Jacobian VectorValues gradientAtZero() const; + /// Raw memory access version of gradientAtZero + virtual void gradientAtZero(double* d) const; + /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ JacobianFactor whiten() const; From cad6736b724b5cd675b62906e139024131146d73 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 17 Nov 2014 16:18:41 -0500 Subject: [PATCH 18/32] Remove non raw memory access functions --- gtsam/slam/RegularHessianFactor.h | 32 +++++++------------ gtsam/slam/RegularImplicitSchurFactor.h | 4 +-- gtsam/slam/RegularJacobianFactor.h | 30 ++++------------- gtsam/slam/tests/testRegularHessianFactor.cpp | 9 ------ .../slam/tests/testRegularJacobianFactor.cpp | 18 +---------- 5 files changed, 21 insertions(+), 72 deletions(-) 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); From 1f827fae43464f8715ebacb4296d05d958701bcd Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 20 Nov 2014 18:59:29 -0500 Subject: [PATCH 19/32] Fix PCG solver parameter initialization --- gtsam/linear/PCGSolver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 27eb57b44..db17f1844 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -33,6 +33,7 @@ void PCGSolverParameters::print(ostream &os) const { /*****************************************************************************/ PCGSolver::PCGSolver(const PCGSolverParameters &p) { + parameters_ = p; preconditioner_ = createPreconditioner(p.preconditioner_); } From 6616844d84a247ab68a3d84bd6a9a50c9fd99e99 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 20 Nov 2014 19:01:02 -0500 Subject: [PATCH 20/32] Fix build function in Preconditioner --- gtsam/linear/Preconditioner.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index c7f4a5b68..7683a0efd 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -143,11 +143,9 @@ void BlockJacobiPreconditioner::build( } /* getting the block diagonals over the factors */ - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { - std::map hessianMap = gf->hessianBlockDiagonal(); - BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) - blocks.push_back(hessian); - } + std::map hessianMap =gfg.hessianBlockDiagonal(); + BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) + blocks.push_back(hessian); /* if necessary, allocating the memory for cacheing the factorization results */ if ( nnz > bufferSize_ ) { From f34c2d941cd448f0f83ea3eecc7529f5b32131bc Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 20 Nov 2014 19:01:41 -0500 Subject: [PATCH 21/32] Add temporary getBuffer function and it should be removed later --- gtsam/linear/Preconditioner.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 10ceb78a9..61df4414d 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -151,6 +151,10 @@ public: const std::map &lambda ) ; + // Should be removed after test + double* getBuffer() { + return buffer_; + } protected: void clean() ; From e72b3465923069ab623b87526b3e60c146e8abc1 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 20 Nov 2014 19:03:20 -0500 Subject: [PATCH 22/32] Add tests for preconditioner and solver --- tests/testPreconditioner.cpp | 176 +++++++++++++++++++++++++++++++++++ 1 file changed, 176 insertions(+) diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 46b243913..4b781201f 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -143,6 +143,182 @@ TEST( Preconditioner, buildBlocks2 ) { EXPECT(assert_equal(it1->second, *it2)); } +/* ************************************************************************* */ +TEST( BlockJacobiPreconditioner, verySimpleLinerSystem) { + // Ax = [4 1][u] = [1] x0 = [2] + // [1 3][v] [2] [1] + // + // exact solution x = [1/11, 7/11]'; + // + + // Create a Gaussian Factor Graph + GaussianFactorGraph simpleGFG; + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3), (Vector(2) << 1,2 ), noiseModel::Unit::Create(2)); + //simpleGFG.print("Factors\n"); + + // Expected Hessian block diagonal matrices + std::map expectedHessian =simpleGFG.hessianBlockDiagonal(); + // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build + std::vector actualHessian = buildBlocks(simpleGFG, KeyInfo(simpleGFG)); + // Compare the number of block diagonal matrices + EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); + + // Compare the values of matrices + std::map::const_iterator it1 = expectedHessian.begin(); + std::vector::const_iterator it2 = actualHessian.begin(); + + // the function 'build' in BlockJacobianPreconditioner stores in 'buffer' + // the cholesky decomposion of each block of the hessian + // In this example there is a single block (i.e., a single value) + // and the corresponding block of the Hessian is + // + // H0 = [17 7; 7 10] + // + EXPECT(assert_equal(it1->second, *it2)); + // TODO: Matrix expectedH0 ... + //EXPECT(assert_equal(it1->second, *it2)); + + // The corresponding Cholesky decomposition is: + // R = chol(H0) = [4.1231 1.6977 0 2.6679] (from Matlab) + Preconditioner::shared_ptr preconditioner = createPreconditioner(boost::make_shared()); + preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map()); + boost::shared_ptr blockJacobi = boost::dynamic_pointer_cast(preconditioner); + double* buf = blockJacobi->getBuffer(); + for(int i=0; i<4; ++i){} + // TODO: EXPECT(assert_equal(number..,buf[i])); + +} +/* ************************************************************************* */ +TEST( PCGsolver, verySimpleLinearSystem) { + + // Ax = [4 1][u] = [1] x0 = [2] + // [1 3][v] [2] [1] + // + // exact solution x = [1/11, 7/11]'; + // + + // Create a Gaussian Factor Graph + GaussianFactorGraph simpleGFG; + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3), (Vector(2) << 1,2 ), noiseModel::Unit::Create(2)); + //simpleGFG.print("Factors\n"); + + // Exact solution already known + VectorValues exactSolution; + exactSolution.insert(0, (Vector(2) << 1./11., 7./11.)); + //exactSolution.print("Exact"); + + // Solve the system using direct method + VectorValues deltaDirect = simpleGFG.optimize(); + EXPECT(assert_equal(exactSolution, deltaDirect, 1e-7)); + //deltaDirect.print("Direct"); + + // Solve the system using PCG + // With Dummy preconditioner + gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + pcg->preconditioner_ = boost::make_shared(); + pcg->setMaxIterations(500); + pcg->setEpsilon_abs(0.0); + pcg->setEpsilon_rel(0.0); + //pcg->setVerbosity("ERROR"); + VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); + EXPECT(assert_equal(exactSolution, deltaPCGDummy, 1e-7)); + + // With Block-Jacobi preconditioner + gtsam::PCGSolverParameters::shared_ptr pcgJacobi = boost::make_shared(); + pcgJacobi->preconditioner_ = boost::make_shared(); + pcgJacobi->setMaxIterations(500); + pcgJacobi->setEpsilon_abs(0.0); + pcgJacobi->setEpsilon_rel(0.0); + VectorValues deltaPCGJacobi = PCGSolver(*pcgJacobi).optimize(simpleGFG); + + // Failed! + EXPECT(assert_equal(exactSolution, deltaPCGJacobi, 1e-5)); + //deltaPCGJacobi.print("PCG Jacobi"); +} + +/* ************************************************************************* */ +TEST(PCGSolver, simpleLinearSystem) { + // Create a Gaussian Factor Graph + GaussianFactorGraph simpleGFG; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << -1, -1), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10), 0, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << 2, -1), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << 0, 1), unit2); + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << -1, 1.5), unit2); + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); + simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); + //simpleGFG.print("Factors\n"); + + // Expected solution + VectorValues expectedSolution; + expectedSolution.insert(0, (Vector(2) << 0.100498, -0.196756)); + expectedSolution.insert(2, (Vector(2) << -0.0990413, -0.0980577)); + expectedSolution.insert(1, (Vector(2) << -0.0973252, 0.100582)); + + // Solve the system using direct method + VectorValues deltaDirect = simpleGFG.optimize(); + EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5)); + //expectedSolution.print("Expected"); + //deltaCholesky.print("Direct"); + + // Solve the system using PCG + VectorValues initial; + initial.insert(0, (Vector(2) << 0.1, -0.1)); + initial.insert(1, (Vector(2) << -0.1, 0.1)); + initial.insert(2, (Vector(2) << -0.1, -0.1)); + + // With Dummy preconditioner + gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + pcg->preconditioner_ = boost::make_shared(); + pcg->setMaxIterations(500); + pcg->setEpsilon_abs(0.0); + pcg->setEpsilon_rel(0.0); + //pcg->setVerbosity("ERROR"); + + VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG, KeyInfo(simpleGFG), std::map(), initial); + // Failed! + EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5)); + //deltaPCGDummy.print("PCG Dummy"); + + // Solve the system using Preconditioned Conjugate Gradient + pcg->preconditioner_ = boost::make_shared(); + VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG, KeyInfo(simpleGFG), std::map(), initial); + // Failed! + EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5)); + //deltaPCGJacobi.print("PCG Jacobi"); + + // Test that the retrieval of the diagonal blocks of the Jacobian are correct. + std::map expectedHessian =simpleGFG.hessianBlockDiagonal(); + std::vector actualHessian = buildBlocks(simpleGFG, KeyInfo(simpleGFG)); + EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); + std::map::const_iterator it1 = expectedHessian.begin(); + std::vector::const_iterator it2 = actualHessian.begin(); + + // The corresponding Cholesky decomposition is: + // R = chol(H0) = [4.1231 1.6977 0 2.6679] (from Matlab) + Preconditioner::shared_ptr preconditioner = createPreconditioner(boost::make_shared()); + preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map()); + boost::shared_ptr blockJacobi = boost::dynamic_pointer_cast(preconditioner); + double* buf = blockJacobi->getBuffer(); + for(int i=0; i<4; ++i){} + // TODO: EXPECT(assert_equal(number..,buf[i])); + + size_t i = 0; + for(; it1!=expectedHessian.end(); it1++, it2++){ + EXPECT(assert_equal(it1->second, *it2)); + Matrix R_i(2,2); + R_i(0,0) = buf[i+0]; + R_i(0,1) = buf[i+1]; + R_i(1,0) = buf[i+2]; + R_i(1,1) = buf[i+3]; + + Matrix actualH_i = R_i.transpose() * R_i;// i-th diagonal block + EXPECT(assert_equal(it1->second, actualH_i)); + i += 4; + } +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From df182b45bcbf96074431fc6fcacfc718d8047c70 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 24 Nov 2014 09:20:43 -0500 Subject: [PATCH 23/32] Add temporary function of getBufferSize which will be removed later --- gtsam/linear/Preconditioner.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 61df4414d..efcb28710 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -155,6 +155,10 @@ public: double* getBuffer() { return buffer_; } + size_t getBufferSize() { + return bufferSize_; + } + protected: void clean() ; From 02131057eea9aff29c86ae1b0843b25c19e984f8 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 24 Nov 2014 09:21:16 -0500 Subject: [PATCH 24/32] Add temporary tests --- tests/testPreconditioner.cpp | 57 ++++++++++++++++++++++++++++++------ 1 file changed, 48 insertions(+), 9 deletions(-) diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 4b781201f..bcb59afe7 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -100,7 +100,7 @@ std::vector buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo & return blocks; } -/* ************************************************************************* */ +/* ************************************************************************* TEST( Preconditioner, buildBlocks ) { // Create simple Gaussian factor graph and initial values GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); @@ -121,7 +121,7 @@ TEST( Preconditioner, buildBlocks ) { EXPECT(assert_equal(it1->second, *it2)); } -/* ************************************************************************* */ +/* ************************************************************************* TEST( Preconditioner, buildBlocks2 ) { // Create simple Gaussian factor graph and initial values GaussianFactorGraph gfg = createSimpleGaussianFactorGraphUnordered(); @@ -143,7 +143,7 @@ TEST( Preconditioner, buildBlocks2 ) { EXPECT(assert_equal(it1->second, *it2)); } -/* ************************************************************************* */ +/* ************************************************************************* TEST( BlockJacobiPreconditioner, verySimpleLinerSystem) { // Ax = [4 1][u] = [1] x0 = [2] // [1 3][v] [2] [1] @@ -168,26 +168,65 @@ TEST( BlockJacobiPreconditioner, verySimpleLinerSystem) { std::vector::const_iterator it2 = actualHessian.begin(); // the function 'build' in BlockJacobianPreconditioner stores in 'buffer' - // the cholesky decomposion of each block of the hessian + // the cholesky decomposion of each block of the hessian (column major) // In this example there is a single block (i.e., a single value) // and the corresponding block of the Hessian is // // H0 = [17 7; 7 10] // - EXPECT(assert_equal(it1->second, *it2)); - // TODO: Matrix expectedH0 ... - //EXPECT(assert_equal(it1->second, *it2)); + Matrix expectedH0 = it1->second; + Matrix actualH0 = *it2; + EXPECT(assert_equal(expectedH0, (Matrix(2,2) << 17, 7, 7, 10) )); + EXPECT(assert_equal(expectedH0, actualH0)); // The corresponding Cholesky decomposition is: // R = chol(H0) = [4.1231 1.6977 0 2.6679] (from Matlab) Preconditioner::shared_ptr preconditioner = createPreconditioner(boost::make_shared()); preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map()); boost::shared_ptr blockJacobi = boost::dynamic_pointer_cast(preconditioner); + Vector expectedR = (Vector(4) << 4.1231, 0, 1.6977, 2.6679); double* buf = blockJacobi->getBuffer(); - for(int i=0; i<4; ++i){} - // TODO: EXPECT(assert_equal(number..,buf[i])); + for(int i=0; i<4; ++i){ + EXPECT_DOUBLES_EQUAL(expectedR(i), buf[i], 1e-4); + } } + +/* ************************************************************************* */ +TEST( BlockJacobiPreconditioner, SimpleLinerSystem) { + // Create a Gaussian Factor Graph + GaussianFactorGraph simpleGFG; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << -1, -1), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10), 0, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << 2, -1), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << 0, 1), unit2); + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << -1, 1.5), unit2); + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); + simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); + + // Expected Hessian block diagonal matrices + std::map expectedHessian =simpleGFG.hessianBlockDiagonal(); + // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build + std::vector actualHessian = buildBlocks(simpleGFG, KeyInfo(simpleGFG)); + // Compare the number of block diagonal matrices + EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); + + // Compare the values of matrices + std::map::const_iterator it1 = expectedHessian.begin(); + std::vector::const_iterator it2 = actualHessian.begin(); + + + Preconditioner::shared_ptr preconditioner = createPreconditioner(boost::make_shared()); + preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map()); + boost::shared_ptr blockJacobi = boost::dynamic_pointer_cast(preconditioner); + double* buf = blockJacobi->getBuffer(); + for(size_t i=0; igetBufferSize(); ++i){ + std::cout << "buf[" << i << "] = " << buf[i] << std::endl; + } + +} + /* ************************************************************************* */ TEST( PCGsolver, verySimpleLinearSystem) { From f6dd14126deef6b795f5b1c4ee56327ce1c0b637 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Sun, 7 Dec 2014 18:26:09 -0500 Subject: [PATCH 25/32] Revert commits related with fixing PCG (reverted from commit 1f827fae43464f8715ebacb4296d05d958701bcd) --- gtsam/linear/PCGSolver.cpp | 1 - gtsam/linear/Preconditioner.cpp | 8 +- gtsam/linear/Preconditioner.h | 8 -- tests/testPreconditioner.cpp | 219 +------------------------------- 4 files changed, 7 insertions(+), 229 deletions(-) diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index db17f1844..27eb57b44 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -33,7 +33,6 @@ void PCGSolverParameters::print(ostream &os) const { /*****************************************************************************/ PCGSolver::PCGSolver(const PCGSolverParameters &p) { - parameters_ = p; preconditioner_ = createPreconditioner(p.preconditioner_); } diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 7683a0efd..c7f4a5b68 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -143,9 +143,11 @@ void BlockJacobiPreconditioner::build( } /* getting the block diagonals over the factors */ - std::map hessianMap =gfg.hessianBlockDiagonal(); - BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) - blocks.push_back(hessian); + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { + std::map hessianMap = gf->hessianBlockDiagonal(); + BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) + blocks.push_back(hessian); + } /* if necessary, allocating the memory for cacheing the factorization results */ if ( nnz > bufferSize_ ) { diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index efcb28710..10ceb78a9 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -151,14 +151,6 @@ public: const std::map &lambda ) ; - // Should be removed after test - double* getBuffer() { - return buffer_; - } - size_t getBufferSize() { - return bufferSize_; - } - protected: void clean() ; diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index bcb59afe7..46b243913 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -100,7 +100,7 @@ std::vector buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo & return blocks; } -/* ************************************************************************* +/* ************************************************************************* */ TEST( Preconditioner, buildBlocks ) { // Create simple Gaussian factor graph and initial values GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); @@ -121,7 +121,7 @@ TEST( Preconditioner, buildBlocks ) { EXPECT(assert_equal(it1->second, *it2)); } -/* ************************************************************************* +/* ************************************************************************* */ TEST( Preconditioner, buildBlocks2 ) { // Create simple Gaussian factor graph and initial values GaussianFactorGraph gfg = createSimpleGaussianFactorGraphUnordered(); @@ -143,221 +143,6 @@ TEST( Preconditioner, buildBlocks2 ) { EXPECT(assert_equal(it1->second, *it2)); } -/* ************************************************************************* -TEST( BlockJacobiPreconditioner, verySimpleLinerSystem) { - // Ax = [4 1][u] = [1] x0 = [2] - // [1 3][v] [2] [1] - // - // exact solution x = [1/11, 7/11]'; - // - - // Create a Gaussian Factor Graph - GaussianFactorGraph simpleGFG; - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3), (Vector(2) << 1,2 ), noiseModel::Unit::Create(2)); - //simpleGFG.print("Factors\n"); - - // Expected Hessian block diagonal matrices - std::map expectedHessian =simpleGFG.hessianBlockDiagonal(); - // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build - std::vector actualHessian = buildBlocks(simpleGFG, KeyInfo(simpleGFG)); - // Compare the number of block diagonal matrices - EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); - - // Compare the values of matrices - std::map::const_iterator it1 = expectedHessian.begin(); - std::vector::const_iterator it2 = actualHessian.begin(); - - // the function 'build' in BlockJacobianPreconditioner stores in 'buffer' - // the cholesky decomposion of each block of the hessian (column major) - // In this example there is a single block (i.e., a single value) - // and the corresponding block of the Hessian is - // - // H0 = [17 7; 7 10] - // - Matrix expectedH0 = it1->second; - Matrix actualH0 = *it2; - EXPECT(assert_equal(expectedH0, (Matrix(2,2) << 17, 7, 7, 10) )); - EXPECT(assert_equal(expectedH0, actualH0)); - - // The corresponding Cholesky decomposition is: - // R = chol(H0) = [4.1231 1.6977 0 2.6679] (from Matlab) - Preconditioner::shared_ptr preconditioner = createPreconditioner(boost::make_shared()); - preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map()); - boost::shared_ptr blockJacobi = boost::dynamic_pointer_cast(preconditioner); - Vector expectedR = (Vector(4) << 4.1231, 0, 1.6977, 2.6679); - double* buf = blockJacobi->getBuffer(); - for(int i=0; i<4; ++i){ - EXPECT_DOUBLES_EQUAL(expectedR(i), buf[i], 1e-4); - } - -} - -/* ************************************************************************* */ -TEST( BlockJacobiPreconditioner, SimpleLinerSystem) { - // Create a Gaussian Factor Graph - GaussianFactorGraph simpleGFG; - SharedDiagonal unit2 = noiseModel::Unit::Create(2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << -1, -1), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10), 0, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << 2, -1), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << 0, 1), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << -1, 1.5), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); - simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); - - // Expected Hessian block diagonal matrices - std::map expectedHessian =simpleGFG.hessianBlockDiagonal(); - // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build - std::vector actualHessian = buildBlocks(simpleGFG, KeyInfo(simpleGFG)); - // Compare the number of block diagonal matrices - EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); - - // Compare the values of matrices - std::map::const_iterator it1 = expectedHessian.begin(); - std::vector::const_iterator it2 = actualHessian.begin(); - - - Preconditioner::shared_ptr preconditioner = createPreconditioner(boost::make_shared()); - preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map()); - boost::shared_ptr blockJacobi = boost::dynamic_pointer_cast(preconditioner); - double* buf = blockJacobi->getBuffer(); - for(size_t i=0; igetBufferSize(); ++i){ - std::cout << "buf[" << i << "] = " << buf[i] << std::endl; - } - -} - -/* ************************************************************************* */ -TEST( PCGsolver, verySimpleLinearSystem) { - - // Ax = [4 1][u] = [1] x0 = [2] - // [1 3][v] [2] [1] - // - // exact solution x = [1/11, 7/11]'; - // - - // Create a Gaussian Factor Graph - GaussianFactorGraph simpleGFG; - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3), (Vector(2) << 1,2 ), noiseModel::Unit::Create(2)); - //simpleGFG.print("Factors\n"); - - // Exact solution already known - VectorValues exactSolution; - exactSolution.insert(0, (Vector(2) << 1./11., 7./11.)); - //exactSolution.print("Exact"); - - // Solve the system using direct method - VectorValues deltaDirect = simpleGFG.optimize(); - EXPECT(assert_equal(exactSolution, deltaDirect, 1e-7)); - //deltaDirect.print("Direct"); - - // Solve the system using PCG - // With Dummy preconditioner - gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); - pcg->preconditioner_ = boost::make_shared(); - pcg->setMaxIterations(500); - pcg->setEpsilon_abs(0.0); - pcg->setEpsilon_rel(0.0); - //pcg->setVerbosity("ERROR"); - VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); - EXPECT(assert_equal(exactSolution, deltaPCGDummy, 1e-7)); - - // With Block-Jacobi preconditioner - gtsam::PCGSolverParameters::shared_ptr pcgJacobi = boost::make_shared(); - pcgJacobi->preconditioner_ = boost::make_shared(); - pcgJacobi->setMaxIterations(500); - pcgJacobi->setEpsilon_abs(0.0); - pcgJacobi->setEpsilon_rel(0.0); - VectorValues deltaPCGJacobi = PCGSolver(*pcgJacobi).optimize(simpleGFG); - - // Failed! - EXPECT(assert_equal(exactSolution, deltaPCGJacobi, 1e-5)); - //deltaPCGJacobi.print("PCG Jacobi"); -} - -/* ************************************************************************* */ -TEST(PCGSolver, simpleLinearSystem) { - // Create a Gaussian Factor Graph - GaussianFactorGraph simpleGFG; - SharedDiagonal unit2 = noiseModel::Unit::Create(2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << -1, -1), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10), 0, (Matrix(2,2)<< 10, 0, 0, 10), (Vector(2) << 2, -1), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << 0, 1), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5), 1, (Matrix(2,2)<< 5, 0, 0, 5), (Vector(2) << -1, 1.5), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); - simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1), (Vector(2) << 0, 0), unit2); - //simpleGFG.print("Factors\n"); - - // Expected solution - VectorValues expectedSolution; - expectedSolution.insert(0, (Vector(2) << 0.100498, -0.196756)); - expectedSolution.insert(2, (Vector(2) << -0.0990413, -0.0980577)); - expectedSolution.insert(1, (Vector(2) << -0.0973252, 0.100582)); - - // Solve the system using direct method - VectorValues deltaDirect = simpleGFG.optimize(); - EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5)); - //expectedSolution.print("Expected"); - //deltaCholesky.print("Direct"); - - // Solve the system using PCG - VectorValues initial; - initial.insert(0, (Vector(2) << 0.1, -0.1)); - initial.insert(1, (Vector(2) << -0.1, 0.1)); - initial.insert(2, (Vector(2) << -0.1, -0.1)); - - // With Dummy preconditioner - gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); - pcg->preconditioner_ = boost::make_shared(); - pcg->setMaxIterations(500); - pcg->setEpsilon_abs(0.0); - pcg->setEpsilon_rel(0.0); - //pcg->setVerbosity("ERROR"); - - VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG, KeyInfo(simpleGFG), std::map(), initial); - // Failed! - EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5)); - //deltaPCGDummy.print("PCG Dummy"); - - // Solve the system using Preconditioned Conjugate Gradient - pcg->preconditioner_ = boost::make_shared(); - VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG, KeyInfo(simpleGFG), std::map(), initial); - // Failed! - EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5)); - //deltaPCGJacobi.print("PCG Jacobi"); - - // Test that the retrieval of the diagonal blocks of the Jacobian are correct. - std::map expectedHessian =simpleGFG.hessianBlockDiagonal(); - std::vector actualHessian = buildBlocks(simpleGFG, KeyInfo(simpleGFG)); - EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); - std::map::const_iterator it1 = expectedHessian.begin(); - std::vector::const_iterator it2 = actualHessian.begin(); - - // The corresponding Cholesky decomposition is: - // R = chol(H0) = [4.1231 1.6977 0 2.6679] (from Matlab) - Preconditioner::shared_ptr preconditioner = createPreconditioner(boost::make_shared()); - preconditioner->build(simpleGFG, KeyInfo(simpleGFG), std::map()); - boost::shared_ptr blockJacobi = boost::dynamic_pointer_cast(preconditioner); - double* buf = blockJacobi->getBuffer(); - for(int i=0; i<4; ++i){} - // TODO: EXPECT(assert_equal(number..,buf[i])); - - size_t i = 0; - for(; it1!=expectedHessian.end(); it1++, it2++){ - EXPECT(assert_equal(it1->second, *it2)); - Matrix R_i(2,2); - R_i(0,0) = buf[i+0]; - R_i(0,1) = buf[i+1]; - R_i(1,0) = buf[i+2]; - R_i(1,1) = buf[i+3]; - - Matrix actualH_i = R_i.transpose() * R_i;// i-th diagonal block - EXPECT(assert_equal(it1->second, actualH_i)); - i += 4; - } -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 0a1e844ae5d1633c62bc67d285cb0ac6930ae0ae Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 8 Dec 2014 00:44:36 -0500 Subject: [PATCH 26/32] Clean up --- gtsam/linear/GaussianFactorGraph.cpp | 51 +++---------------- gtsam/linear/GaussianFactorGraph.h | 4 -- .../linear/tests/testGaussianFactorGraph.cpp | 26 ---------- 3 files changed, 8 insertions(+), 73 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index ad43b675b..4dd1af4e7 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -355,14 +355,6 @@ namespace gtsam { 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); - //} - /* ************************************************************************* */ void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const { multiplyInPlace(x, e.begin()); @@ -392,42 +384,15 @@ namespace gtsam { /* ************************************************************************* */ // x += alpha*A'*e -void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, - VectorValues& x) const { - // For each factor add the gradient contribution - Errors::const_iterator ei = e.begin(); - BOOST_FOREACH(const sharedFactor& Ai_G, *this) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - Ai->transposeMultiplyAdd(alpha, *(ei++), x); + void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, + VectorValues& x) const { + // For each factor add the gradient contribution + Errors::const_iterator ei = e.begin(); + BOOST_FOREACH(const sharedFactor& Ai_G, *this) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + Ai->transposeMultiplyAdd(alpha, *(ei++), x); + } } -} - - ///* ************************************************************************* */ - //void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { - // Key i = 0 ; - // BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - // r[i] = Ai->getb(); - // i++; - // } - // VectorValues Ax = VectorValues::SameStructure(r); - // multiply(fg,x,Ax); - // axpy(-1.0,Ax,r); - //} - - ///* ************************************************************************* */ - //void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { - // r.setZero(); - // Key i = 0; - // BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - // Vector &y = r[i]; - // for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { - // y += Ai->getA(j) * x[*j]; - // } - // ++i; - // } - //} /* ************************************************************************* */ VectorValues GaussianFactorGraph::transposeMultiply(const Errors& e) const diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 1861e2607..9b8a91a6e 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -310,10 +310,6 @@ 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/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 2fc1e359b..49a6e30e4 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -315,31 +315,6 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 ) EXPECT(assert_equal(2*expected, actual)); } -/* ************************************************************************* */ -// Raw memory access -//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]; - -// Vector fast_y = gtsam::zero(6); -// 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 ) { @@ -352,7 +327,6 @@ TEST( GaussianFactorGraph, matricesMixed ) EXPECT(assert_equal(A.transpose()*b, eta)); } - /* ************************************************************************* */ TEST( GaussianFactorGraph, gradientAtZero ) { From d64af0d626f7a44c59b65b5603cb3d7ce61365c2 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Sat, 13 Dec 2014 17:02:38 -0500 Subject: [PATCH 27/32] Modify gradientAtZero in RegularJacobianFactor --- gtsam/slam/RegularJacobianFactor.h | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index a9759cc26..e6e240389 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -143,19 +143,30 @@ public: /** Raw memory access version of gradientAtZero */ 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; - // Just iterate over all A matrices + // Initialize d as a zero vector for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { DMap(d + D * j) = DVector::Zero(); + } + + // Get vector b not weighted + Vector b = getb(); + + // Whitening b + if (model_) { + b = model_->whiten(b); + b = model_->whiten(b); + } + + // Just iterate over all A matrices + for (size_t pos = 0; pos < size(); ++pos) { + Key j = keys_[pos]; 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); + Vector column_k = Ab_(pos).col(k); + dj(k) = -1.0*dot(b, column_k); } DMap(d + D * j) += dj; } From 1affae697c574e778388ffb65a1a3c1f2e43d36e Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 22 Dec 2014 14:50:26 -0500 Subject: [PATCH 28/32] Correct gradientAtZero: remove zero initialization --- gtsam/slam/RegularJacobianFactor.h | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index e6e240389..bb275ef3f 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -141,14 +141,11 @@ public: } } - /** Raw memory access version of gradientAtZero */ + /** Raw memory access version of gradientAtZero + * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n + */ virtual void gradientAtZero(double* d) const { - // Initialize d as a zero vector - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - DMap(d + D * j) = DVector::Zero(); - } - // Get vector b not weighted Vector b = getb(); @@ -159,13 +156,12 @@ public: } // Just iterate over all A matrices - for (size_t pos = 0; pos < size(); ++pos) { - Key j = keys_[pos]; + for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { DVector dj; // gradient -= A'*b/sigma^2 // Computing with each column for (size_t k = 0; k < D; ++k){ - Vector column_k = Ab_(pos).col(k); + Vector column_k = Ab_(j).col(k); dj(k) = -1.0*dot(b, column_k); } DMap(d + D * j) += dj; From 82a2d7f029fbeeb247e18dc5f63c08467e4ab648 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 22 Dec 2014 14:51:23 -0500 Subject: [PATCH 29/32] Add a unit test for gradientAtZero with multi-factors --- .../slam/tests/testRegularJacobianFactor.cpp | 48 ++++++++++++++++++- 1 file changed, 46 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp index c19f6db8f..16273d23f 100644 --- a/gtsam/slam/tests/testRegularJacobianFactor.cpp +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -38,6 +38,17 @@ namespace { const Vector b = (Vector(3) << 1., 2., 3.); const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5,0.5,0.5)); } + + namespace simple2 { + // Terms + const vector > terms2 = list_of > + (make_pair(0, 2*Matrix3::Identity())) + (make_pair(1, 4*Matrix3::Identity())) + (make_pair(2, 6*Matrix3::Identity())); + + // RHS + const Vector b2 = (Vector(3) << 2., 4., 6.); + } } /* ************************************************************************* */ @@ -99,7 +110,7 @@ TEST(RegularJacobianFactor, hessianDiagonal) VectorValues expectedHessianDiagonal = factor.hessianDiagonal(); // we compare against the Raw memory access implementation of hessianDiagonal - double actualValue[9]; + double actualValue[9]={0}; regularFactor.hessianDiagonal(actualValue); VectorValues actualHessianDiagonalRaw = double2vv(actualValue,nrKeys,fixedDim); EXPECT(assert_equal(expectedHessianDiagonal, actualHessianDiagonalRaw)); @@ -119,12 +130,45 @@ TEST(RegularJacobian, gradientAtZero) //EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero())); // we compare against the Raw memory access implementation of gradientAtZero - double actualValue[9]; + double actualValue[9]={0}; regularFactor.gradientAtZero(actualValue); VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim); EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw)); } +/* ************************************************************************* */ +TEST(RegularJacobian, gradientAtZero_multiFactors) +{ + using namespace simple; + 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); + + // we compute gradient at zero from the standard Jacobian + VectorValues expectedGradientAtZero = factor.gradientAtZero(); + + // we compare against the Raw memory access implementation of gradientAtZero + double actualValue[9]={0}; + regularFactor.gradientAtZero(actualValue); + VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim); + EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw)); + + // One more factor + using namespace simple2; + JacobianFactor factor2(terms2[0].first, terms2[0].second, + terms2[1].first, terms2[1].second, terms2[2].first, terms2[2].second, b2, noise); + RegularJacobianFactor regularFactor2(terms2, b2, noise); + + // we accumulate computed gradient at zero from the standard Jacobian + VectorValues expectedGradientAtZero2 = expectedGradientAtZero.add(factor2.gradientAtZero()); + + // we compare against the Raw memory access implementation of gradientAtZero + regularFactor2.gradientAtZero(actualValue); + VectorValues actualGradientAtZeroRaw2 = double2vv(actualValue,nrKeys,fixedDim); + EXPECT(assert_equal(expectedGradientAtZero2, actualGradientAtZeroRaw2)); + +} + /* ************************************************************************* */ TEST(RegularJacobian, multiplyHessianAdd) { From 7e0033208c2d3b635dbd14246e869ec7fcd254f8 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Mon, 29 Dec 2014 14:48:48 -0500 Subject: [PATCH 30/32] JacobianSchurFactor is merged into RegularJacobianFactor. Derived classes from JacobianSchurFactor are changed to be derived from RegularJacobianFactor. --- gtsam/slam/JacobianFactorQ.h | 18 +++--- gtsam/slam/JacobianFactorQR.h | 16 +++-- gtsam/slam/JacobianFactorSVD.h | 12 ++-- gtsam/slam/JacobianSchurFactor.h | 93 ------------------------------ gtsam/slam/RegularJacobianFactor.h | 57 +++++++++++++++--- 5 files changed, 77 insertions(+), 119 deletions(-) delete mode 100644 gtsam/slam/JacobianSchurFactor.h diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index f4dfb9422..957041abc 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -6,16 +6,20 @@ #pragma once -#include "JacobianSchurFactor.h" +#include namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQ: public JacobianSchurFactor { +class JacobianFactorQ: public RegularJacobianFactor { - typedef JacobianSchurFactor Base; +private: + + typedef RegularJacobianFactor Base; + typedef Eigen::Matrix Matrix2D; + typedef std::pair KeyMatrix2D; public: @@ -25,7 +29,7 @@ public: /// Empty constructor with keys JacobianFactorQ(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor() { Matrix zeroMatrix = Matrix::Zero(0,D); Vector zeroVector = Vector::Zero(0); typedef std::pair KeyMatrix; @@ -37,10 +41,10 @@ public: } /// Constructor - JacobianFactorQ(const std::vector& Fblocks, + JacobianFactorQ(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + RegularJacobianFactor() { size_t j = 0, m2 = E.rows(), m = m2 / 2; // Calculate projector Q Matrix Q = eye(m2) - E * P * E.transpose(); @@ -50,7 +54,7 @@ public: std::vector < KeyMatrix > QF; QF.reserve(m); // Below, we compute each 2m*D block A_j = Q_j * F_j = (2m*2) * (2*D) - BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) + BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // Which is then passed to the normal JacobianFactor constructor JacobianFactor::fillTerms(QF, Q * b, model); diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index a928106a8..30faf9ec0 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -6,31 +6,35 @@ */ #pragma once -#include +#include namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQR: public JacobianSchurFactor { +class JacobianFactorQR: public RegularJacobianFactor { - typedef JacobianSchurFactor Base; +private: + + typedef RegularJacobianFactor Base; + typedef Eigen::Matrix Matrix2D; + typedef std::pair KeyMatrix2D; public: /** * Constructor */ - JacobianFactorQR(const std::vector& Fblocks, + JacobianFactorQR(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + RegularJacobianFactor() { // Create a number of Jacobian factors in a factor graph GaussianFactorGraph gfg; Symbol pointKey('p', 0); size_t i = 0; - BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) { + BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) { gfg.add(pointKey, E.block<2, 3>(2 * i, 0), it.first, it.second, b.segment<2>(2 * i), model); i += 1; diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index e28185038..0f4c52038 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -5,27 +5,29 @@ */ #pragma once -#include "gtsam/slam/JacobianSchurFactor.h" +#include namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorSVD: public JacobianSchurFactor { +class JacobianFactorSVD: public RegularJacobianFactor { -public: +private: typedef Eigen::Matrix Matrix2D; typedef std::pair KeyMatrix2D; typedef std::pair KeyMatrix; +public: + /// Default constructor JacobianFactorSVD() {} /// Empty constructor with keys JacobianFactorSVD(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor() { Matrix zeroMatrix = Matrix::Zero(0,D); Vector zeroVector = Vector::Zero(0); std::vector QF; @@ -37,7 +39,7 @@ public: /// Constructor JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor() { size_t numKeys = Enull.rows() / 2; size_t j = 0, m2 = 2*numKeys-3; // PLAIN NULL SPACE TRICK diff --git a/gtsam/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h deleted file mode 100644 index 2beecc264..000000000 --- a/gtsam/slam/JacobianSchurFactor.h +++ /dev/null @@ -1,93 +0,0 @@ -/* - * @file JacobianSchurFactor.h - * @brief Jacobianfactor that combines and eliminates points - * @date Oct 27, 2013 - * @uthor Frank Dellaert - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace gtsam { -/** - * JacobianFactor for Schur complement that uses Q noise model - */ -template -class JacobianSchurFactor: public JacobianFactor { - -public: - - typedef Eigen::Matrix Matrix2D; - typedef std::pair KeyMatrix2D; - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - - /** - * @brief double* Matrix-vector multiply, i.e. y = A*x - * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way - */ - Vector operator*(const double* x) const { - Vector Ax = zero(Ab_.rows()); - if (empty()) return Ax; - - // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhiten(Ax) : Ax; - } - - /** - * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e - * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way - */ - void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const - { - Vector E = alpha * (model_ ? model_->whiten(e) : e); - // Just iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; postransposeMultiplyAdd(alpha,Ax,y); - 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 +#include #include #include #include @@ -30,6 +32,9 @@ class RegularJacobianFactor: public JacobianFactor { private: + typedef JacobianFactor Base; + typedef RegularJacobianFactor This; + /** Use eigen magic to access raw memory */ typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; @@ -37,6 +42,10 @@ private: public: + /// Default constructor + RegularJacobianFactor() { + } + /** Construct an n-ary factor * @tparam TERMS A container whose value type is std::pair, specifying the * collection of keys and matrices making up the factor. */ @@ -57,6 +66,33 @@ public: JacobianFactor(keys, augmentedMatrix, sigmas) { } + /** + * @brief double* Matrix-vector multiply, i.e. y = A*x + * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way + */ + Vector operator*(const double* x) const { + Vector Ax = zero(Ab_.rows()); + if (empty()) return Ax; + + // Just iterate over all A matrices and multiply in correct config part + for(size_t pos=0; poswhiten(Ax) : Ax; + } + + /** + * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e + * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way + */ + void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const + { + Vector E = alpha * (model_ ? model_->whiten(e) : e); + // Just iterate over all A matrices and insert Ai^e into y + for(size_t pos=0; pos& accumulatedDims) const { - /// Use eigen magic to access raw memory - typedef Eigen::Matrix VectorD; - typedef Eigen::Map MapD; - typedef Eigen::Map ConstMapD; - if (empty()) return; Vector Ax = zero(Ab_.rows()); @@ -81,7 +112,7 @@ public: for (size_t pos = 0; pos < size(); ++pos) { Ax += Ab_(pos) - * ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[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_) { @@ -94,12 +125,15 @@ public: /// Again iterate over all A matrices and insert Ai^T into y for (size_t pos = 0; pos < size(); ++pos){ - MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( + DMap(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( pos).transpose() * Ax; } } - /** Raw memory access version of multiplyHessianAdd */ + /** + * @brief double* Hessian-vector multiply, i.e. y += A'*(A*x) + * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way + */ void multiplyHessianAdd(double alpha, const double* x, double* y) const { if (empty()) return; @@ -168,6 +202,13 @@ public: } } + /** Non-RAW memory access versions for testRegularImplicitSchurFactor + * TODO: They should be removed from Regular Factors + */ + using Base::multiplyHessianAdd; + using Base::hessianDiagonal; + using Base::gradientAtZero; + }; } From 7091c2bd2ebc1f216c9b31e6fb14afe2a29ba553 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 1 Jan 2015 17:35:58 -0500 Subject: [PATCH 31/32] To resolve conflicts first, revert the commitment of merging JacobianSchurFactor into RegularJacobianFactor. JacobianSchurFactor is merged into RegularJacobianFactor. Derived classes from JacobianSchurFactor are changed to be derived from RegularJacobianFactor. (reverted from commit 7e0033208c2d3b635dbd14246e869ec7fcd254f8) --- gtsam/slam/JacobianFactorQ.h | 18 +++--- gtsam/slam/JacobianFactorQR.h | 16 ++--- gtsam/slam/JacobianFactorSVD.h | 12 ++-- gtsam/slam/JacobianSchurFactor.h | 93 ++++++++++++++++++++++++++++++ gtsam/slam/RegularJacobianFactor.h | 57 +++--------------- 5 files changed, 119 insertions(+), 77 deletions(-) create mode 100644 gtsam/slam/JacobianSchurFactor.h diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index 957041abc..f4dfb9422 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -6,20 +6,16 @@ #pragma once -#include +#include "JacobianSchurFactor.h" namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQ: public RegularJacobianFactor { +class JacobianFactorQ: public JacobianSchurFactor { -private: - - typedef RegularJacobianFactor Base; - typedef Eigen::Matrix Matrix2D; - typedef std::pair KeyMatrix2D; + typedef JacobianSchurFactor Base; public: @@ -29,7 +25,7 @@ public: /// Empty constructor with keys JacobianFactorQ(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor() { + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { Matrix zeroMatrix = Matrix::Zero(0,D); Vector zeroVector = Vector::Zero(0); typedef std::pair KeyMatrix; @@ -41,10 +37,10 @@ public: } /// Constructor - JacobianFactorQ(const std::vector& Fblocks, + JacobianFactorQ(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : - RegularJacobianFactor() { + JacobianSchurFactor() { size_t j = 0, m2 = E.rows(), m = m2 / 2; // Calculate projector Q Matrix Q = eye(m2) - E * P * E.transpose(); @@ -54,7 +50,7 @@ public: std::vector < KeyMatrix > QF; QF.reserve(m); // Below, we compute each 2m*D block A_j = Q_j * F_j = (2m*2) * (2*D) - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) + BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // Which is then passed to the normal JacobianFactor constructor JacobianFactor::fillTerms(QF, Q * b, model); diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index 30faf9ec0..a928106a8 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -6,35 +6,31 @@ */ #pragma once -#include +#include namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQR: public RegularJacobianFactor { +class JacobianFactorQR: public JacobianSchurFactor { -private: - - typedef RegularJacobianFactor Base; - typedef Eigen::Matrix Matrix2D; - typedef std::pair KeyMatrix2D; + typedef JacobianSchurFactor Base; public: /** * Constructor */ - JacobianFactorQR(const std::vector& Fblocks, + JacobianFactorQR(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : - RegularJacobianFactor() { + JacobianSchurFactor() { // Create a number of Jacobian factors in a factor graph GaussianFactorGraph gfg; Symbol pointKey('p', 0); size_t i = 0; - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) { + BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) { gfg.add(pointKey, E.block<2, 3>(2 * i, 0), it.first, it.second, b.segment<2>(2 * i), model); i += 1; diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index 0f4c52038..e28185038 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -5,29 +5,27 @@ */ #pragma once -#include +#include "gtsam/slam/JacobianSchurFactor.h" namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorSVD: public RegularJacobianFactor { +class JacobianFactorSVD: public JacobianSchurFactor { -private: +public: typedef Eigen::Matrix Matrix2D; typedef std::pair KeyMatrix2D; typedef std::pair KeyMatrix; -public: - /// Default constructor JacobianFactorSVD() {} /// Empty constructor with keys JacobianFactorSVD(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor() { + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { Matrix zeroMatrix = Matrix::Zero(0,D); Vector zeroVector = Vector::Zero(0); std::vector QF; @@ -39,7 +37,7 @@ public: /// Constructor JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor() { + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { size_t numKeys = Enull.rows() / 2; size_t j = 0, m2 = 2*numKeys-3; // PLAIN NULL SPACE TRICK diff --git a/gtsam/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h new file mode 100644 index 000000000..2beecc264 --- /dev/null +++ b/gtsam/slam/JacobianSchurFactor.h @@ -0,0 +1,93 @@ +/* + * @file JacobianSchurFactor.h + * @brief Jacobianfactor that combines and eliminates points + * @date Oct 27, 2013 + * @uthor Frank Dellaert + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { +/** + * JacobianFactor for Schur complement that uses Q noise model + */ +template +class JacobianSchurFactor: public JacobianFactor { + +public: + + typedef Eigen::Matrix Matrix2D; + typedef std::pair KeyMatrix2D; + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + + /** + * @brief double* Matrix-vector multiply, i.e. y = A*x + * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way + */ + Vector operator*(const double* x) const { + Vector Ax = zero(Ab_.rows()); + if (empty()) return Ax; + + // Just iterate over all A matrices and multiply in correct config part + for(size_t pos=0; poswhiten(Ax) : Ax; + } + + /** + * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e + * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way + */ + void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const + { + Vector E = alpha * (model_ ? model_->whiten(e) : e); + // Just iterate over all A matrices and insert Ai^e into y + for(size_t pos=0; postransposeMultiplyAdd(alpha,Ax,y); + 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 -#include #include #include #include @@ -32,9 +30,6 @@ class RegularJacobianFactor: public JacobianFactor { private: - typedef JacobianFactor Base; - typedef RegularJacobianFactor This; - /** Use eigen magic to access raw memory */ typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; @@ -42,10 +37,6 @@ private: public: - /// Default constructor - RegularJacobianFactor() { - } - /** Construct an n-ary factor * @tparam TERMS A container whose value type is std::pair, specifying the * collection of keys and matrices making up the factor. */ @@ -66,33 +57,6 @@ public: JacobianFactor(keys, augmentedMatrix, sigmas) { } - /** - * @brief double* Matrix-vector multiply, i.e. y = A*x - * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way - */ - Vector operator*(const double* x) const { - Vector Ax = zero(Ab_.rows()); - if (empty()) return Ax; - - // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhiten(Ax) : Ax; - } - - /** - * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e - * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way - */ - void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const - { - Vector E = alpha * (model_ ? model_->whiten(e) : e); - // Just iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; pos& accumulatedDims) const { + /// Use eigen magic to access raw memory + typedef Eigen::Matrix VectorD; + typedef Eigen::Map MapD; + typedef Eigen::Map ConstMapD; + if (empty()) return; Vector Ax = zero(Ab_.rows()); @@ -112,7 +81,7 @@ public: for (size_t pos = 0; pos < size(); ++pos) { Ax += Ab_(pos) - * ConstDMap(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); + * ConstMapD(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_) { @@ -125,15 +94,12 @@ public: /// 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_( + MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( pos).transpose() * Ax; } } - /** - * @brief double* Hessian-vector multiply, i.e. y += A'*(A*x) - * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way - */ + /** Raw memory access version of multiplyHessianAdd */ void multiplyHessianAdd(double alpha, const double* x, double* y) const { if (empty()) return; @@ -202,13 +168,6 @@ public: } } - /** Non-RAW memory access versions for testRegularImplicitSchurFactor - * TODO: They should be removed from Regular Factors - */ - using Base::multiplyHessianAdd; - using Base::hessianDiagonal; - using Base::gradientAtZero; - }; } From fcd705f716bf53184a1ceea06e69eabefde4dd55 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 1 Jan 2015 18:12:45 -0500 Subject: [PATCH 32/32] Correct Eigen comma initialization --- gtsam/slam/tests/testRegularHessianFactor.cpp | 24 +++++++++---------- .../slam/tests/testRegularJacobianFactor.cpp | 18 +++++++------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp index 7e7f877f4..8dfb753f4 100644 --- a/gtsam/slam/tests/testRegularHessianFactor.cpp +++ b/gtsam/slam/tests/testRegularHessianFactor.cpp @@ -34,18 +34,18 @@ const double tol = 1e-5; /* ************************************************************************* */ TEST(RegularHessianFactor, ConstructorNWay) { - Matrix G11 = (Matrix(2,2) << 111, 112, 113, 114); - Matrix G12 = (Matrix(2,2) << 121, 122, 123, 124); - Matrix G13 = (Matrix(2,2) << 131, 132, 133, 134); + Matrix G11 = (Matrix(2,2) << 111, 112, 113, 114).finished(); + Matrix G12 = (Matrix(2,2) << 121, 122, 123, 124).finished(); + Matrix G13 = (Matrix(2,2) << 131, 132, 133, 134).finished(); - Matrix G22 = (Matrix(2,2) << 221, 222, 222, 224); - Matrix G23 = (Matrix(2,2) << 231, 232, 233, 234); + Matrix G22 = (Matrix(2,2) << 221, 222, 222, 224).finished(); + Matrix G23 = (Matrix(2,2) << 231, 232, 233, 234).finished(); - Matrix G33 = (Matrix(2,2) << 331, 332, 332, 334); + Matrix G33 = (Matrix(2,2) << 331, 332, 332, 334).finished(); - Vector g1 = (Vector(2) << -7, -9); - Vector g2 = (Vector(2) << -9, 1); - Vector g3 = (Vector(2) << 2, 3); + Vector g1 = (Vector(2) << -7, -9).finished(); + Vector g2 = (Vector(2) << -9, 1).finished(); + Vector g3 = (Vector(2) << 2, 3).finished(); double f = 10; @@ -68,9 +68,9 @@ TEST(RegularHessianFactor, ConstructorNWay) EXPECT(assert_equal(Y,AtA*X)); VectorValues x = map_list_of - (0, (Vector(2) << 1,2)) - (1, (Vector(2) << 3,4)) - (3, (Vector(2) << 5,6)); + (0, (Vector(2) << 1,2).finished()) + (1, (Vector(2) << 3,4).finished()) + (3, (Vector(2) << 5,6).finished()); VectorValues expected; expected.insert(0, Y.segment<2>(0)); diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp index 16273d23f..b56b65d7b 100644 --- a/gtsam/slam/tests/testRegularJacobianFactor.cpp +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -35,8 +35,8 @@ namespace { (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 Vector b = (Vector(3) << 1., 2., 3.).finished(); + const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5,0.5,0.5).finished()); } namespace simple2 { @@ -47,7 +47,7 @@ namespace { (make_pair(2, 6*Matrix3::Identity())); // RHS - const Vector b2 = (Vector(3) << 2., 4., 6.); + const Vector b2 = (Vector(3) << 2., 4., 6.).finished(); } } @@ -179,15 +179,15 @@ TEST(RegularJacobian, multiplyHessianAdd) // arbitrary vector X VectorValues X; - X.insert(0, (Vector(3) << 10.,20.,30.)); - X.insert(1, (Vector(3) << 10.,20.,30.)); - X.insert(2, (Vector(3) << 10.,20.,30.)); + X.insert(0, (Vector(3) << 10.,20.,30.).finished()); + X.insert(1, (Vector(3) << 10.,20.,30.).finished()); + X.insert(2, (Vector(3) << 10.,20.,30.).finished()); // arbitrary vector Y VectorValues Y; - Y.insert(0, (Vector(3) << 10.,10.,10.)); - Y.insert(1, (Vector(3) << 20.,20.,20.)); - Y.insert(2, (Vector(3) << 30.,30.,30.)); + Y.insert(0, (Vector(3) << 10.,10.,10.).finished()); + Y.insert(1, (Vector(3) << 20.,20.,20.).finished()); + Y.insert(2, (Vector(3) << 30.,30.,30.).finished()); // multiplyHessianAdd Y += alpha*A'A*X double alpha = 2.0;