From 0d05bf5ac53c33f457e05cc7bfa9a946cb492bc0 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 1 Aug 2013 21:57:43 +0000 Subject: [PATCH] Started on HessianFactor, converted Nonlinear stuff and disabled a lot of more advanced classes so the library compiles with nonlinear using unordered. --- CMakeLists.txt | 5 +- gtsam/base/SymmetricBlockMatrix.cpp | 48 ++ gtsam/base/SymmetricBlockMatrix.h | 229 +++++++ gtsam/base/VerticalBlockMatrix.cpp | 7 +- gtsam/base/VerticalBlockMatrix.h | 103 ++- gtsam/linear/GaussianFactor.h | 6 +- gtsam/linear/HessianFactor.cpp | 539 +++++++++++++++ gtsam/linear/HessianFactor.h | 402 +++++++++++ gtsam/linear/HessianFactorOrdered.cpp | 18 +- gtsam/linear/HessianFactorOrdered.h | 16 +- gtsam/linear/JacobianFactor-inl.h | 6 +- gtsam/linear/JacobianFactor.cpp | 47 +- gtsam/linear/JacobianFactor.h | 31 +- gtsam/linear/VectorValues.cpp | 4 +- gtsam/linear/VectorValues.h | 6 +- ...ctor.cpp => testHessianFactorObsolete.cpp} | 0 .../tests/testHessianFactorUnordered.cpp | 638 ++++++++++++++++++ gtsam/nonlinear/DoglegOptimizer.cpp | 4 + gtsam/nonlinear/DoglegOptimizer.h | 4 + gtsam/nonlinear/GaussNewtonOptimizer.h | 2 +- gtsam/nonlinear/ISAM2-impl.cpp | 4 + gtsam/nonlinear/ISAM2-impl.h | 4 + gtsam/nonlinear/ISAM2.cpp | 4 + gtsam/nonlinear/ISAM2.h | 4 + .../nonlinear/LevenbergMarquardtOptimizer.cpp | 5 + gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 8 +- gtsam/nonlinear/LinearContainerFactor.cpp | 18 +- gtsam/nonlinear/LinearContainerFactor.h | 12 +- gtsam/nonlinear/Marginals.cpp | 4 + gtsam/nonlinear/Marginals.h | 4 + .../NonlinearConjugateGradientOptimizer.cpp | 4 + .../NonlinearConjugateGradientOptimizer.h | 9 +- gtsam/nonlinear/NonlinearFactor.h | 2 +- gtsam/nonlinear/NonlinearFactorGraph.h | 26 +- gtsam/nonlinear/NonlinearISAM.cpp | 4 + gtsam/nonlinear/NonlinearISAM.h | 3 + .../SuccessiveLinearizationOptimizer.cpp | 18 +- .../SuccessiveLinearizationOptimizer.h | 14 +- gtsam/nonlinear/Values.cpp | 2 +- gtsam/nonlinear/summarization.cpp | 3 + gtsam/nonlinear/summarization.h | 4 +- gtsam/slam/dataset.cpp | 6 +- 42 files changed, 2103 insertions(+), 174 deletions(-) create mode 100644 gtsam/base/SymmetricBlockMatrix.cpp create mode 100644 gtsam/base/SymmetricBlockMatrix.h create mode 100644 gtsam/linear/HessianFactor.cpp create mode 100644 gtsam/linear/HessianFactor.h rename gtsam/linear/tests/{testHessianFactor.cpp => testHessianFactorObsolete.cpp} (100%) create mode 100644 gtsam/linear/tests/testHessianFactorUnordered.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 74044ec68..cd1fafdcd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -103,6 +103,7 @@ endif() if(MSVC AND NOT Boost_USE_STATIC_LIBS) add_definitions(-DBOOST_ALL_DYN_LINK) endif() +add_definitions(-DBOOST_ALL_NO_LIB) find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time regex timer chrono) @@ -114,7 +115,9 @@ endif() option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) # Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library) -set(GTSAM_BOOST_LIBRARIES ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY}) +set(GTSAM_BOOST_LIBRARIES + ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} + ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY}) if (GTSAM_DISABLE_NEW_TIMERS) message("WARNING: GTSAM timing instrumentation manually disabled") add_definitions(-DGTSAM_DISABLE_NEW_TIMERS) diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp new file mode 100644 index 000000000..02f8b3fc0 --- /dev/null +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -0,0 +1,48 @@ +/* ---------------------------------------------------------------------------- + +* 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 SymmetricBlockMatrix.cpp +* @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional. +* @author Richard Roberts +* @date Sep 18, 2010 +*/ + +#include +#include + +namespace gtsam { + + /* ************************************************************************* */ + SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(const SymmetricBlockMatrix& other) + { + SymmetricBlockMatrix result; + result.variableColOffsets_.resize(other.nBlocks() + 1); + std::copy(other.variableColOffsets_.begin() + other.blockStart_, other.variableColOffsets_.end(), + result.variableColOffsets_.begin()); + result.matrix_.resize(other.cols(), other.cols()); + result.assertInvariants(); + return result; + } + + /* ************************************************************************* */ + SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& other) + { + SymmetricBlockMatrix result; + result.variableColOffsets_.resize(other.nBlocks() + 1); + std::copy(other.variableColOffsets_.begin() + other.blockStart_, other.variableColOffsets_.end(), + result.variableColOffsets_.begin()); + result.matrix_.resize(other.cols(), other.cols()); + result.assertInvariants(); + return result; + } + +} diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h new file mode 100644 index 000000000..281418c90 --- /dev/null +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -0,0 +1,229 @@ +/* ---------------------------------------------------------------------------- + +* 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 SymmetricBlockMatrix.h +* @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional. +* @author Richard Roberts +* @date Sep 18, 2010 +*/ +#pragma once + +#include + +namespace gtsam { + + // Forward declarations + class VerticalBlockMatrix; + + /** + * This class stores a dense matrix and allows it to be accessed as a collection of blocks. When + * constructed, the caller must provide the dimensions of the blocks. + * + * The block structure is symmetric, but the underlying matrix does not necessarily need to be. + * + * This class also has a parameter that can be changed after construction to change the apparent + * matrix view. firstBlock() determines the block that appears to have index 0 for all operations + * (except re-setting firstBlock()). + * + * @addtogroup base */ + class SymmetricBlockMatrix + { + public: + typedef SymmetricBlockMatrix This; + typedef Eigen::Block Block; + typedef Eigen::Block constBlock; + + protected: + Matrix matrix_; ///< The full matrix + std::vector variableColOffsets_; ///< the starting columns of each block (0-based) + + DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment. + + public: + /** Construct from an empty matrix (asserts that the matrix is empty) */ + SymmetricBlockMatrix() : + blockStart_(0) + { + variableColOffsets_.push_back(0); + assertInvariants(); + } + + /** Construct from a container of the sizes of each block. */ + template + SymmetricBlockMatrix(const CONTAINER dimensions) : + blockStart_(0) + { + fillOffsets(dimensions.begin(), dimensions.end()); + matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); + assertInvariants(); + } + + /** + * Construct from iterator over the sizes of each vertical block. */ + template + SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim) : + blockStart_(0) + { + fillOffsets(firstBlockDim, lastBlockDim); + matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); + assertInvariants(); + } + + /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */ + template + SymmetricBlockMatrix(const CONTAINER dimensions, const Matrix& matrix) : + matrix_(matrix), blockStart_(0) + { + fillOffsets(dimensions.begin(), dimensions.end()); + if(matrix_.rows() != matrix_.cols()) + throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from a non-square matrix."); + if(variableColOffsets_.back() != matrix_.cols()) + throw std::invalid_argument("Requested to create a SymmetricBlockMatrix with dimensions that do not sum to the total size of the provided matrix."); + assertInvariants(); + } + + /** Copy the block structure, but do not copy the matrix data. If blockStart() has been + * modified, this copies the structure of the corresponding matrix view. In the destination + * SymmetricBlockMatrix, blockStart() will be 0. */ + static SymmetricBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& other); + + /** Copy the block structure, but do not copy the matrix data. If blockStart() has been + * modified, this copies the structure of the corresponding matrix view. In the destination + * SymmetricBlockMatrix, blockStart() will be 0. */ + static SymmetricBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& other); + + /** Row size */ + DenseIndex rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } + + /** Column size */ + DenseIndex cols() const { return rows(); } + + + /** Block count */ + DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; } + + Block operator()(DenseIndex i_block, DenseIndex j_block) { + return range(i_block, i_block+1, j_block, j_block+1); + } + + constBlock operator()(DenseIndex i_block, DenseIndex j_block) const { + return range(i_block, i_block+1, j_block, j_block+1); + } + + Block range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) { + assertInvariants(); + DenseIndex i_actualStartBlock = i_startBlock + blockStart_; + DenseIndex i_actualEndBlock = i_endBlock + blockStart_; + DenseIndex j_actualStartBlock = j_startBlock + blockStart_; + DenseIndex j_actualEndBlock = j_endBlock + blockStart_; + checkBlock(i_actualStartBlock); + checkBlock(j_actualStartBlock); + if(i_startBlock != 0 || i_endBlock != 0) { + checkBlock(i_actualStartBlock); + assert(i_actualEndBlock < (DenseIndex)variableColOffsets_.size()); + } + if(j_startBlock != 0 || j_endBlock != 0) { + checkBlock(j_actualStartBlock); + assert(j_actualEndBlock < (DenseIndex)variableColOffsets_.size()); + } + return matrix_.block( + variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock], + variableColOffsets_[i_actualEndBlock] - variableColOffsets_[i_actualStartBlock], + variableColOffsets_[j_actualEndBlock] - variableColOffsets_[j_actualStartBlock]); + } + + constBlock range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const { + assertInvariants(); + DenseIndex i_actualStartBlock = i_startBlock + blockStart_; + DenseIndex i_actualEndBlock = i_endBlock + blockStart_; + DenseIndex j_actualStartBlock = j_startBlock + blockStart_; + DenseIndex j_actualEndBlock = j_endBlock + blockStart_; + checkBlock(i_actualStartBlock); + checkBlock(j_actualStartBlock); + if(i_startBlock != 0 || i_endBlock != 0) { + checkBlock(i_actualStartBlock); + assert(i_actualEndBlock < (DenseIndex)variableColOffsets_.size()); + } + if(j_startBlock != 0 || j_endBlock != 0) { + checkBlock(j_actualStartBlock); + assert(j_actualEndBlock < (DenseIndex)variableColOffsets_.size()); + } + return matrix_.block( + variableColOffsets_[i_actualStartBlock], variableColOffsets_[j_actualStartBlock], + variableColOffsets_[i_actualEndBlock] - variableColOffsets_[i_actualStartBlock], + variableColOffsets_[j_actualEndBlock] - variableColOffsets_[j_actualStartBlock]); + } + + /** Return the full matrix, *not* including any portions excluded by firstBlock(). */ + Block full() { + return range(0,nBlocks(), 0,nBlocks()); + } + + /** Return the full matrix, *not* including any portions excluded by firstBlock(). */ + constBlock full() const { + return range(0,nBlocks(), 0,nBlocks()); + } + + /** Access to full matrix, including any portions excluded by firstBlock() to other operations. */ + const Matrix& matrix() const { return matrix_; } + + /** Access to full matrix, including any portions excluded by firstBlock() to other operations. */ + Matrix& matrix() { return matrix_; } + + DenseIndex offset(DenseIndex block) const { + assertInvariants(); + DenseIndex actualBlock = block + blockStart_; + checkBlock(actualBlock); + return variableColOffsets_[actualBlock]; + } + + DenseIndex& blockStart() { return blockStart_; } + DenseIndex blockStart() const { return blockStart_; } + + protected: + void assertInvariants() const { + assert(matrix_.rows() == matrix_.cols()); + assert(matrix_.cols() == variableColOffsets_.back()); + assert(blockStart_ < (DenseIndex)variableColOffsets_.size()); + } + + void checkBlock(DenseIndex block) const { + assert(matrix_.rows() == matrix_.cols()); + assert(matrix_.cols() == variableColOffsets_.back()); + assert(block < (DenseIndex)variableColOffsets_.size()-1); + assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols()); + } + + template + void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) { + variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1); + variableColOffsets_[0] = 0; + DenseIndex j=0; + for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { + variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; + ++ j; + } + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(matrix_); + ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); + ar & BOOST_SERIALIZATION_NVP(blockStart_); + } + }; + + +} diff --git a/gtsam/base/VerticalBlockMatrix.cpp b/gtsam/base/VerticalBlockMatrix.cpp index 1c75387a3..1eb8c01a7 100644 --- a/gtsam/base/VerticalBlockMatrix.cpp +++ b/gtsam/base/VerticalBlockMatrix.cpp @@ -21,11 +21,12 @@ namespace gtsam { /* ************************************************************************* */ - VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& rhs) { + VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& rhs) + { VerticalBlockMatrix result; result.variableColOffsets_.resize(rhs.nBlocks() + 1); - for(Index i = 0; i < rhs.nBlocks() + 1; ++i) - result.variableColOffsets_[i] = rhs.variableColOffsets_[i + rhs.blockStart_]; + std::copy(rhs.variableColOffsets_.begin() + rhs.blockStart_, rhs.variableColOffsets_.end(), + result.variableColOffsets_.begin()); result.matrix_.resize(rhs.rows(), result.variableColOffsets_.back()); result.rowEnd_ = rhs.rows(); result.assertInvariants(); diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index 8772487df..11c4d14d2 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -12,7 +12,7 @@ /** * @file VerticalBlockMatrix.h * @brief A matrix with column blocks of pre-defined sizes. Used in JacobianFactor and - * GaussianConditional. + * GaussianConditional. * @author Richard Roberts * @date Sep 18, 2010 */ #pragma once @@ -22,40 +22,35 @@ namespace gtsam { // Forward declarations - //class S + class SymmetricBlockMatrix; /** - * This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks. - * It also provides for accessing individual columns from those blocks. When constructed or - * resized, the caller must provide the dimensions of the blocks. + * This class stores a dense matrix and allows it to be accessed as a collection of vertical + * blocks. The dimensions of the blocks are provided when constructing this class. * * This class also has three parameters that can be changed after construction that change the * apparent view of the matrix without any reallocation or data copying. firstBlock() determines * the block that has index 0 for all operations (except for re-setting firstBlock()). rowStart() - * determines the apparent first row of the matrix for all operations (except for setting rowStart() - * and rowEnd()). rowEnd() determines the apparent exclusive (one-past-the-last) last row for all - * operations. To include all rows, rowEnd() should be set to the number of rows in the matrix - * (i.e. one after the last true row index). + * determines the apparent first row of the matrix for all operations (except for setting + * rowStart() and rowEnd()). rowEnd() determines the apparent exclusive (one-past-the-last) last + * row for all operations. To include all rows, rowEnd() should be set to the number of rows in + * the matrix (i.e. one after the last true row index). * * @addtogroup base */ - class GTSAM_EXPORT VerticalBlockMatrix { + class GTSAM_EXPORT VerticalBlockMatrix + { public: typedef VerticalBlockMatrix This; - typedef Matrix::Index Index; typedef Eigen::Block Block; typedef Eigen::Block constBlock; - // columns of blocks - typedef Eigen::Block::ColXpr Column; - typedef Eigen::Block::ConstColXpr constColumn; - protected: Matrix matrix_; ///< The full matrix - std::vector variableColOffsets_; ///< the starting columns of each block (0-based) + std::vector variableColOffsets_; ///< the starting columns of each block (0-based) - Index rowStart_; ///< Changes apparent matrix view, see main class comment. - Index rowEnd_; ///< Changes apparent matrix view, see main class comment. - Index blockStart_; ///< Changes apparent matrix view, see main class comment. + DenseIndex rowStart_; ///< Changes apparent matrix view, see main class comment. + DenseIndex rowEnd_; ///< Changes apparent matrix view, see main class comment. + DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment. public: @@ -64,10 +59,10 @@ namespace gtsam { rowStart_(0), rowEnd_(0), blockStart_(0) { variableColOffsets_.push_back(0); + assertInvariants(); } - /** Construct from a container of the sizes of each vertical block, resize the matrix so that - * its height is matrixNewHeight and its width fits the given block dimensions. */ + /** Construct from a container of the sizes of each vertical block. */ template VerticalBlockMatrix(const CONTAINER dimensions, DenseIndex height) : rowStart_(0), rowEnd_(height), blockStart_(0) @@ -89,8 +84,7 @@ namespace gtsam { } /** - * Construct from iterator over the sizes of each vertical block, resize the matrix so that its - * height is matrixNewHeight and its width fits the given block dimensions. */ + * Construct from iterator over the sizes of each vertical block. */ template VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height) : rowStart_(0), rowEnd_(height), blockStart_(0) @@ -101,44 +95,44 @@ namespace gtsam { } /// Row size - Index rows() const { assertInvariants(); return rowEnd_ - rowStart_; } + DenseIndex rows() const { assertInvariants(); return rowEnd_ - rowStart_; } /// Column size - Index cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } + DenseIndex cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } /// Block count - Index nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; } + DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; } /** Access a single block in the underlying matrix with read/write access */ - Block operator()(Index block) { return range(block, block+1); } + Block operator()(DenseIndex block) { return range(block, block+1); } /** Access a const block view */ - const constBlock operator()(Index block) const { return range(block, block+1); } + const constBlock operator()(DenseIndex block) const { return range(block, block+1); } /** access ranges of blocks at a time */ - Block range(Index startBlock, Index endBlock) { + Block range(DenseIndex startBlock, DenseIndex endBlock) { assertInvariants(); - Index actualStartBlock = startBlock + blockStart_; - Index actualEndBlock = endBlock + blockStart_; + DenseIndex actualStartBlock = startBlock + blockStart_; + DenseIndex actualEndBlock = endBlock + blockStart_; if(startBlock != 0 || endBlock != 0) { checkBlock(actualStartBlock); assert(actualEndBlock < (DenseIndex)variableColOffsets_.size()); } - const Index startCol = variableColOffsets_[actualStartBlock]; - const Index rangeCols = variableColOffsets_[actualEndBlock] - startCol; + const DenseIndex startCol = variableColOffsets_[actualStartBlock]; + const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol; return matrix_.block(rowStart_, startCol, this->rows(), rangeCols); } - const constBlock range(Index startBlock, Index endBlock) const { + const constBlock range(DenseIndex startBlock, DenseIndex endBlock) const { assertInvariants(); - Index actualStartBlock = startBlock + blockStart_; - Index actualEndBlock = endBlock + blockStart_; + DenseIndex actualStartBlock = startBlock + blockStart_; + DenseIndex actualEndBlock = endBlock + blockStart_; if(startBlock != 0 || endBlock != 0) { checkBlock(actualStartBlock); assert(actualEndBlock < (DenseIndex)variableColOffsets_.size()); } - const Index startCol = variableColOffsets_[actualStartBlock]; - const Index rangeCols = variableColOffsets_[actualEndBlock] - startCol; + const DenseIndex startCol = variableColOffsets_[actualStartBlock]; + const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol; return ((const Matrix&)matrix_).block(rowStart_, startCol, this->rows(), rangeCols); } @@ -148,30 +142,30 @@ namespace gtsam { /** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */ const constBlock full() const { return range(0, nBlocks()); } - Index offset(Index block) const { + DenseIndex offset(DenseIndex block) const { assertInvariants(); - Index actualBlock = block + blockStart_; + DenseIndex actualBlock = block + blockStart_; checkBlock(actualBlock); return variableColOffsets_[actualBlock]; } /** Get or set the apparent first row of the underlying matrix for all operations */ - Index& rowStart() { return rowStart_; } + DenseIndex& rowStart() { return rowStart_; } /** Get or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ - Index& rowEnd() { return rowEnd_; } + DenseIndex& rowEnd() { return rowEnd_; } /** Get or set the apparent first block for all operations */ - Index& firstBlock() { return blockStart_; } + DenseIndex& firstBlock() { return blockStart_; } /** Get the apparent first row of the underlying matrix for all operations */ - Index rowStart() const { return rowStart_; } + DenseIndex rowStart() const { return rowStart_; } /** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ - Index rowEnd() const { return rowEnd_; } + DenseIndex rowEnd() const { return rowEnd_; } /** Get the apparent first block for all operations */ - Index firstBlock() const { return blockStart_; } + DenseIndex firstBlock() const { return blockStart_; } /** Access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */ const Matrix& matrix() const { return matrix_; } @@ -179,14 +173,11 @@ namespace gtsam { /** Non-const access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */ Matrix& matrix() { return matrix_; } - /** - * Copy the block structure and resize the underlying matrix, but do not - * copy the matrix data. If blockStart(), rowStart(), and/or rowEnd() have - * been modified, this copies the structure of the corresponding matrix view. - * In the destination VerticalBlockView, blockStart() and rowStart() will - * thus be 0, rowEnd() will be cols() of the source VerticalBlockView, and - * the underlying matrix will be the size of the view of the source matrix. - */ + /** Copy the block structure and resize the underlying matrix, but do not copy the matrix data. + * If blockStart(), rowStart(), and/or rowEnd() have been modified, this copies the structure of + * the corresponding matrix view. In the destination VerticalBlockView, blockStart() and + * rowStart() will thus be 0, rowEnd() will be cols() of the source VerticalBlockView, and the + * underlying matrix will be the size of the view of the source matrix. */ static VerticalBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& rhs); protected: @@ -208,14 +199,14 @@ namespace gtsam { void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) { variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1); variableColOffsets_[0] = 0; - Index j=0; + DenseIndex j=0; for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; ++ j; } } - //friend class SymmetricBlockView; + friend class SymmetricBlockMatrix; private: /** Serialization function */ diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 9c87682e3..95364ea87 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -67,7 +67,7 @@ namespace gtsam { * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian. */ - virtual Matrix augmentedJacobian(bool weight = true) const = 0; + virtual Matrix augmentedJacobian() const = 0; /** * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$, @@ -76,7 +76,7 @@ namespace gtsam { * GaussianFactorGraph::augmentedJacobian and * GaussianFactorGraph::sparseJacobian. */ - virtual std::pair jacobian(bool weight = true) const = 0; + virtual std::pair jacobian() const = 0; /** Return the augmented information matrix represented by this GaussianFactor. * The augmented information matrix contains the information matrix with an @@ -104,7 +104,7 @@ namespace gtsam { * stored stored in this factor. * @return a HessianFactor with negated Hessian matrices */ - //virtual GaussianFactor::shared_ptr negate() const = 0; + virtual GaussianFactor::shared_ptr negate() const = 0; private: /** Serialization function */ diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp new file mode 100644 index 000000000..cc6ded573 --- /dev/null +++ b/gtsam/linear/HessianFactor.cpp @@ -0,0 +1,539 @@ +/* ---------------------------------------------------------------------------- + + * 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 HessianFactor.cpp + * @author Richard Roberts + * @date Dec 8, 2010 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#endif +#include +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif +#include +#include +#include + +#include + +using namespace std; +using namespace boost::assign; +using boost::adaptors::transformed; +namespace br = boost::range; + +namespace gtsam { + +/* ************************************************************************* */ +string SlotEntry::toString() const { + ostringstream oss; + oss << "SlotEntry: slot=" << slot << ", dim=" << dimension; + return oss.str(); +} + +/* ************************************************************************* */ +Scatter::Scatter(const GaussianFactorGraph& gfg) { + // First do the set union. + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) { + if(factor) { + for(GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) { + this->insert(make_pair(*variable, SlotEntry(0, factor->getDim(variable)))); + } + } + } + + // Next fill in the slot indices (we can only get these after doing the set + // union. + size_t slot = 0; + BOOST_FOREACH(value_type& var_slot, *this) { + var_slot.second.slot = (slot ++); + } +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor() : + info_(cref_list_of<1>(1)) +{ + linearTerm().setZero(); + constantTerm() = 0.0; +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) : + GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) +{ + if(G.rows() != G.cols() || G.rows() != g.size()) throw invalid_argument( + "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); + info_(0,0) = G; + info_(0,1) = g; + info_(1,1)(0,0) = f; +} + +/* ************************************************************************* */ +// error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu) +// where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g +HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) : + GaussianFactor(cref_list_of<1>(j)), + info_(cref_list_of<2> (Sigma.cols()) (1) ) +{ + if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument( + "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); + info_(0,0) = Sigma.inverse(); // G + info_(0,1) = info_(0,0) * mu; // g + info_(1,1)(0,0) = mu.dot(info_(0,1).col(0)); // f +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor(Key j1, Key j2, + const Matrix& G11, const Matrix& G12, const Vector& g1, + const Matrix& G22, const Vector& g2, double f) : +GaussianFactor(cref_list_of<2>(j1)(j2)), + info_(cref_list_of<3> (G11.cols()) (G22.cols()) (1) ) +{ + info_(0,0) = G11; + info_(0,1) = G12; + info_(0,2) = g1; + info_(1,1) = G22; + info_(1,2) = g2; + info_(2,2)(0,0) = f; +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor(Key j1, Key j2, Key j3, + const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, + const Matrix& G22, const Matrix& G23, const Vector& g2, + const Matrix& G33, const Vector& g3, double f) : +GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), + info_(cref_list_of<4> (G11.cols()) (G22.cols()) (G33.cols()) (1) ) +{ + if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() || + G22.cols() != G12.cols() || G33.cols() != G13.cols() || G22.cols() != g2.size() || G33.cols() != g3.size()) + throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + info_(0,0) = G11; + info_(0,1) = G12; + info_(0,2) = G13; + info_(0,3) = g1; + info_(1,1) = G22; + info_(1,2) = G23; + info_(1,3) = g2; + info_(2,2) = G33; + info_(2,3) = g3; + info_(3,3)(0,0) = f; +} + +/* ************************************************************************* */ +namespace { DenseIndex _getColsHF(const Matrix& m) { return m.cols(); } } + +/* ************************************************************************* */ +HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, + const std::vector& gs, double f) : +GaussianFactor(js), info_(br::join(Gs | transformed(&_getColsHF), cref_list_of<1,DenseIndex>(1))) +{ + // Get the number of variables + size_t variable_count = js.size(); + + // Verify the provided number of entries in the vectors are consistent + if(gs.size() != variable_count || Gs.size() != (variable_count*(variable_count+1))/2) + throw invalid_argument("Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \ + in js must match the number of linear vector pieces in gs. The number of upper-diagonal blocks in Gs must be n*(n+1)/2"); + + // Verify the dimensions of each provided matrix are consistent + // Note: equations for calculating the indices derived from the "sum of an arithmetic sequence" formula + for(size_t i = 0; i < variable_count; ++i){ + DenseIndex block_size = gs[i].size(); + // Check rows + for(size_t j = 0; j < variable_count-i; ++j){ + size_t index = i*(2*variable_count - i + 1)/2 + j; + if(Gs[index].rows() != block_size){ + throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + } + } + // Check cols + for(size_t j = 0; j <= i; ++j){ + size_t index = j*(2*variable_count - j + 1)/2 + (i-j); + if(Gs[index].cols() != block_size){ + throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + } + } + } + + // Fill in the blocks + size_t index = 0; + for(size_t i = 0; i < variable_count; ++i){ + for(size_t j = i; j < variable_count; ++j){ + info_(i, j) = Gs[index++]; + } + info_(i, variable_count) = gs[i]; + } + info_(variable_count, variable_count)(0,0) = f; +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor(const JacobianFactor& jf) +{ + throw std::runtime_error("Not implemented"); +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor(const GaussianFactor& gf) +{ + throw std::runtime_error("Not implemented"); + //// Copy the variable indices + //(GaussianFactorOrdered&)(*this) = gf; + //// Copy the matrix data depending on what type of factor we're copying from + //if(dynamic_cast(&gf)) { + // const JacobianFactorOrdered& jf(static_cast(gf)); + // if(jf.model_->isConstrained()) + // throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); + // else { + // Vector invsigmas = jf.model_->invsigmas().cwiseProduct(jf.model_->invsigmas()); + // info_.copyStructureFrom(jf.Ab_); + // BlockInfo::constBlock A = jf.Ab_.full(); + // matrix_.noalias() = A.transpose() * invsigmas.asDiagonal() * A; + // } + //} else if(dynamic_cast(&gf)) { + // const HessianFactorOrdered& hf(static_cast(gf)); + // info_.assignNoalias(hf.info_); + //} else + // throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor"); + //assertInvariants(); +} + +/* ************************************************************************* */ +HessianFactor::HessianFactor(const GaussianFactorGraph& factors) +{ + throw std::runtime_error("Not implemented"); + + //Scatter scatter(factors); + + //// Pull out keys and dimensions + //gttic(keys); + //vector dimensions(scatter.size() + 1); + //BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { + // dimensions[var_slot.second.slot] = var_slot.second.dimension; + //} + //// This is for the r.h.s. vector + //dimensions.back() = 1; + //gttoc(keys); + + //const bool debug = ISDEBUG("EliminateCholesky"); + //// Form Ab' * Ab + //gttic(allocate); + //info_.resize(dimensions.begin(), dimensions.end(), false); + //// Fill in keys + //keys_.resize(scatter.size()); + //std::transform(scatter.begin(), scatter.end(), keys_.begin(), boost::bind(&Scatter::value_type::first, ::_1)); + //gttoc(allocate); + //gttic(zero); + //matrix_.noalias() = Matrix::Zero(matrix_.rows(),matrix_.cols()); + //gttoc(zero); + //gttic(update); + //if (debug) cout << "Combining " << factors.size() << " factors" << endl; + //BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factors) + //{ + // if(factor) { + // if(shared_ptr hessian = boost::dynamic_pointer_cast(factor)) + // updateATA(*hessian, scatter); + // else if(JacobianFactorOrdered::shared_ptr jacobianFactor = boost::dynamic_pointer_cast(factor)) + // updateATA(*jacobianFactor, scatter); + // else + // throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); + // } + //} + //gttoc(update); + + //if (debug) gtsam::print(matrix_, "Ab' * Ab: "); + + //assertInvariants(); +} + +/* ************************************************************************* */ +void HessianFactor::print(const std::string& s, const KeyFormatter& formatter) const { + cout << s << "\n"; + cout << " keys: "; + for(const_iterator key=this->begin(); key!=this->end(); ++key) + cout << formatter(*key) << "(" << this->getDim(key) << ") "; + cout << "\n"; + gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView()), "Ab^T * Ab: "); +} + +/* ************************************************************************* */ +bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { + if(!dynamic_cast(&lf)) + return false; + else { + Matrix thisMatrix = this->info_.full().selfadjointView(); + thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; + Matrix rhsMatrix = static_cast(lf).info_.full().selfadjointView(); + rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; + return equal_with_abs_tol(thisMatrix, rhsMatrix, tol); + } +} + +/* ************************************************************************* */ +Matrix HessianFactor::augmentedInformation() const { + return info_.full().selfadjointView(); +} + +/* ************************************************************************* */ +Matrix HessianFactor::information() const { + return info_.range(0, this->size(), 0, this->size()).selfadjointView(); +} + +/* ************************************************************************* */ +Matrix HessianFactor::augmentedJacobian() const +{ + throw std::runtime_error("Not implemented"); +} + +/* ************************************************************************* */ +std::pair HessianFactor::jacobian() const +{ + throw std::runtime_error("Not implemented"); +} + +/* ************************************************************************* */ +double HessianFactor::error(const VectorValues& c) const { + // error 0.5*(f - 2*x'*g + x'*G*x) + const double f = constantTerm(); + double xtg = 0, xGx = 0; + // extract the relevant subset of the VectorValues + // NOTE may not be as efficient + const Vector x = c.vector(this->keys()); + xtg = x.dot(linearTerm()); + xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x; + return 0.5 * (f - 2.0 * xtg + xGx); +} + +/* ************************************************************************* */ +void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) +{ + + throw std::runtime_error("Not implemented"); + + //// This function updates 'combined' with the information in 'update'. + //// 'scatter' maps variables in the update factor to slots in the combined + //// factor. + + //const bool debug = ISDEBUG("updateATA"); + + //// First build an array of slots + //gttic(slots); + //size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google. + //size_t slot = 0; + //BOOST_FOREACH(Index j, update) { + // slots[slot] = scatter.find(j)->second.slot; + // ++ slot; + //} + //gttoc(slots); + + //if(debug) { + // this->print("Updating this: "); + // update.print("with (Hessian): "); + //} + + //// Apply updates to the upper triangle + //gttic(update); + //for(size_t j2=0; j2info_.nBlocks()-1 : slots[j2]; + // for(size_t j1=0; j1<=j2; ++j1) { + // size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1]; + // if(slot2 > slot1) { + // if(debug) + // cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; + // matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() += + // update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()); + // } else if(slot1 > slot2) { + // if(debug) + // cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl; + // matrix_.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() += + // update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()).transpose(); + // } else { + // if(debug) + // cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; + // matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).triangularView() += + // update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()); + // } + // if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n"; + // if(debug) this->print(); + // } + //} + //gttoc(update); +} + +/* ************************************************************************* */ +void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatter) +{ + throw std::runtime_error("Not implemented"); + + //// This function updates 'combined' with the information in 'update'. + //// 'scatter' maps variables in the update factor to slots in the combined + //// factor. + + //const bool debug = ISDEBUG("updateATA"); + + //// First build an array of slots + //gttic(slots); + //size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google. + //size_t slot = 0; + //BOOST_FOREACH(Index j, update) { + // slots[slot] = scatter.find(j)->second.slot; + // ++ slot; + //} + //gttoc(slots); + + //gttic(form_ATA); + //if(update.model_->isConstrained()) + // throw invalid_argument("Cannot update HessianFactor from JacobianFactor with constrained noise model"); + + //if(debug) { + // this->print("Updating this: "); + // update.print("with (Jacobian): "); + //} + + //typedef Eigen::Block BlockUpdateMatrix; + //BlockUpdateMatrix updateA(update.matrix_.block( + // update.Ab_.rowStart(),update.Ab_.offset(0), update.Ab_.full().rows(), update.Ab_.full().cols())); + //if (debug) cout << "updateA: \n" << updateA << endl; + + //Matrix updateInform; + //if(boost::dynamic_pointer_cast(update.model_)) { + // updateInform.noalias() = updateA.transpose() * updateA; + //} else { + // noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast(update.model_)); + // if(diagonal) { + // Vector invsigmas2 = update.model_->invsigmas().cwiseProduct(update.model_->invsigmas()); + // updateInform.noalias() = updateA.transpose() * invsigmas2.asDiagonal() * updateA; + // } else + // throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal"); + //} + //if (debug) cout << "updateInform: \n" << updateInform << endl; + // gttoc(form_ATA); + + //// Apply updates to the upper triangle + //gttic(update); + //for(size_t j2=0; j2info_.nBlocks()-1 : slots[j2]; + // for(size_t j1=0; j1<=j2; ++j1) { + // size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1]; + // size_t off0 = update.Ab_.offset(0); + // if(slot2 > slot1) { + // if(debug) + // cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; + // matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() += + // updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()); + // } else if(slot1 > slot2) { + // if(debug) + // cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl; + // matrix_.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() += + // updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()).transpose(); + // } else { + // if(debug) + // cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; + // matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).triangularView() += + // updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()); + // } + // if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n"; + // if(debug) this->print(); + // } + //} + //gttoc(update); +} + +/* ************************************************************************* */ +void HessianFactor::partialCholesky(size_t nrFrontals) +{ + throw std::runtime_error("Not implemented"); + + //if(!choleskyPartial(matrix_, info_.offset(nrFrontals))) + // throw IndeterminantLinearSystemException(this->keys().front()); +} + +/* ************************************************************************* */ +GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFrontals) +{ + throw std::runtime_error("Not implemented"); + + //static const bool debug = false; + + //// Extract conditionals + //gttic(extract_conditionals); + //GaussianConditionalOrdered::shared_ptr conditional(new GaussianConditionalOrdered()); + //typedef VerticalBlockView BlockAb; + //BlockAb Ab(matrix_, info_); + + //size_t varDim = info_.offset(nrFrontals); + //Ab.rowEnd() = Ab.rowStart() + varDim; + + //// Create one big conditionals with many frontal variables. + //gttic(construct_cond); + //Vector sigmas = Vector::Ones(varDim); + //conditional = boost::make_shared(keys_.begin(), keys_.end(), nrFrontals, Ab, sigmas); + //gttoc(construct_cond); + //if(debug) conditional->print("Extracted conditional: "); + + //gttoc(extract_conditionals); + + //// Take lower-right block of Ab_ to get the new factor + //gttic(remaining_factor); + //info_.blockStart() = nrFrontals; + //// Assign the keys + //vector remainingKeys(keys_.size() - nrFrontals); + //remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end()); + //keys_.swap(remainingKeys); + //gttoc(remaining_factor); + + //return conditional; +} + +/* ************************************************************************* */ +GaussianFactor::shared_ptr HessianFactor::negate() const +{ + throw std::runtime_error("Not implemented"); + + //// Copy Hessian Blocks from Hessian factor and invert + //std::vector js; + //std::vector Gs; + //std::vector gs; + //double f; + //js.insert(js.end(), begin(), end()); + //for(size_t i = 0; i < js.size(); ++i){ + // for(size_t j = i; j < js.size(); ++j){ + // Gs.push_back( -info(begin()+i, begin()+j) ); + // } + // gs.push_back( -linearTerm(begin()+i) ); + //} + //f = -constantTerm(); + + //// Create the Anti-Hessian Factor from the negated blocks + //return HessianFactorOrdered::shared_ptr(new HessianFactorOrdered(js, Gs, gs, f)); +} + +} // gtsam diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h new file mode 100644 index 000000000..7e1da53f2 --- /dev/null +++ b/gtsam/linear/HessianFactor.h @@ -0,0 +1,402 @@ +/* ---------------------------------------------------------------------------- + + * 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 HessianFactor.h + * @brief Contains the HessianFactor class, a general quadratic factor + * @author Richard Roberts + * @date Dec 8, 2010 + */ + +#pragma once + +#include +#include + +#include + +// Forward declarations for friend unit tests +class HessianFactorConversionConstructorTest; +class HessianFactorConstructor1Test; +class HessianFactorConstructor1bTest; +class HessianFactorcombineTest; + + +namespace gtsam { + + // Forward declarations + class JacobianFactor; + class GaussianConditional; + class GaussianBayesNet; + + // Definition of Scatter, which is an intermediate data structure used when + // building a HessianFactor incrementally, to get the keys in the right + // order. + // The "scatter" is a map from global variable indices to slot indices in the + // union of involved variables. We also include the dimensionality of the + // variable. + struct GTSAM_EXPORT SlotEntry { + size_t slot; + size_t dimension; + SlotEntry(size_t _slot, size_t _dimension) + : slot(_slot), dimension(_dimension) {} + std::string toString() const; + }; + class Scatter : public FastMap { + public: + Scatter() {} + Scatter(const GaussianFactorGraph& gfg); + }; + + /** + * @brief A Gaussian factor using the canonical parameters (information form) + * + * HessianFactor implements a general quadratic factor of the form + * \f[ E(x) = 0.5 x^T G x - x^T g + 0.5 f \f] + * that stores the matrix \f$ G \f$, the vector \f$ g \f$, and the constant term \f$ f \f$. + * + * When \f$ G \f$ is positive semidefinite, this factor represents a Gaussian, + * in which case \f$ G \f$ is the information matrix \f$ \Lambda \f$, + * \f$ g \f$ is the information vector \f$ \eta \f$, and \f$ f \f$ is the residual + * sum-square-error at the mean, when \f$ x = \mu \f$. + * + * Indeed, the negative log-likelihood of a Gaussian is (up to a constant) + * @f$ E(x) = 0.5(x-\mu)^T P^{-1} (x-\mu) @f$ + * with @f$ \mu @f$ the mean and @f$ P @f$ the covariance matrix. Expanding the product we get + * @f[ + * E(x) = 0.5 x^T P^{-1} x - x^T P^{-1} \mu + 0.5 \mu^T P^{-1} \mu + * @f] + * We define the Information matrix (or Hessian) @f$ \Lambda = P^{-1} @f$ + * and the information vector @f$ \eta = P^{-1} \mu = \Lambda \mu @f$ + * to arrive at the canonical form of the Gaussian: + * @f[ + * E(x) = 0.5 x^T \Lambda x - x^T \eta + 0.5 \mu^T \Lambda \mu + * @f] + * + * This factor is one of the factors that can be in a GaussianFactorGraph. + * It may be returned from NonlinearFactor::linearize(), but is also + * used internally to store the Hessian during Cholesky elimination. + * + * This can represent a quadratic factor with characteristics that cannot be + * represented using a JacobianFactor (which has the form + * \f$ E(x) = \Vert Ax - b \Vert^2 \f$ and stores the Jacobian \f$ A \f$ + * and error vector \f$ b \f$, i.e. is a sum-of-squares factor). For example, + * a HessianFactor need not be positive semidefinite, it can be indefinite or + * even negative semidefinite. + * + * If a HessianFactor is indefinite or negative semi-definite, then in order + * for solving the linear system to be possible, + * the Hessian of the full system must be positive definite (i.e. when all + * small Hessians are combined, the result must be positive definite). If + * this is not the case, an error will occur during elimination. + * + * This class stores G, g, and f as an augmented matrix HessianFactor::matrix_. + * The upper-left n x n blocks of HessianFactor::matrix_ store the upper-right + * triangle of G, the upper-right-most column of length n of HessianFactor::matrix_ + * stores g, and the lower-right entry of HessianFactor::matrix_ stores f, i.e. + * \code + HessianFactor::matrix_ = [ G11 G12 G13 ... g1 + 0 G22 G23 ... g2 + 0 0 G33 ... g3 + : : : : + 0 0 0 ... f ] + \endcode + Blocks can be accessed as follows: + \code + G11 = info(begin(), begin()); + G12 = info(begin(), begin()+1); + G23 = info(begin()+1, begin()+2); + g2 = linearTerm(begin()+1); + f = constantTerm(); + ....... + \endcode + */ + class GTSAM_EXPORT HessianFactor : public GaussianFactor { + protected: + + SymmetricBlockMatrix info_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1] + + public: + + typedef GaussianFactor Base; ///< Typedef to base class + typedef HessianFactor This; ///< Typedef to this class + typedef boost::shared_ptr shared_ptr; ///< A shared_ptr to this class + typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix + typedef SymmetricBlockMatrix::constBlock constBlock; ///< A block from the Hessian matrix (const version) + + /** default constructor for I/O */ + HessianFactor(); + + /** Construct a unary factor. G is the quadratic term (Hessian matrix), g + * the linear term (a vector), and f the constant term. The quadratic + * error is: + * 0.5*(f - 2*x'*g + x'*G*x) + */ + HessianFactor(Index j, const Matrix& G, const Vector& g, double f); + + /** Construct a unary factor, given a mean and covariance matrix. + * error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) + */ + HessianFactor(Index j, const Vector& mu, const Matrix& Sigma); + + /** Construct a binary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. + * JacobianFactor error is \f[ 0.5* (Ax-b)' M (Ax-b) = 0.5*x'A'MAx - x'A'Mb + 0.5*b'Mb \f] + * HessianFactor error is \f[ 0.5*(x'Gx - 2x'g + f) = 0.5*x'Gx - x'*g + 0.5*f \f] + * So, with \f$ A = [A1 A2] \f$ and \f$ G=A*'M*A = [A1';A2']*M*[A1 A2] \f$ we have + \code + n1*n1 G11 = A1'*M*A1 + n1*n2 G12 = A1'*M*A2 + n2*n2 G22 = A2'*M*A2 + n1*1 g1 = A1'*M*b + n2*1 g2 = A2'*M*b + 1*1 f = b'*M*b + \endcode + */ + HessianFactor(Index j1, Index j2, + const Matrix& G11, const Matrix& G12, const Vector& g1, + const Matrix& G22, const Vector& g2, double f); + + /** Construct a ternary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. + */ + HessianFactor(Index j1, Index j2, Index j3, + const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, + const Matrix& G22, const Matrix& G23, const Vector& g2, + const Matrix& G33, const Vector& g3, double f); + + /** Construct an n-way factor. Gs contains the upper-triangle blocks of the + * quadratic term (the Hessian matrix) provided in row-order, gs the pieces + * of the linear vector term, and f the constant term. + */ + HessianFactor(const std::vector& js, const std::vector& Gs, + const std::vector& gs, double f); + + /** Construct from a JacobianFactor (or from a GaussianConditional since it derives from it) */ + explicit HessianFactor(const JacobianFactor& cg); + + /** Attempt to construct from any GaussianFactor - currently supports JacobianFactor, + * HessianFactor, GaussianConditional, or any derived classes. */ + explicit HessianFactor(const GaussianFactor& factor); + + /** Combine a set of factors into a single dense HessianFactor */ + HessianFactor(const GaussianFactorGraph& factors); + + /** Destructor */ + virtual ~HessianFactor() {} + + /** Clone this HessianFactor */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::make_shared(*this); } + + /** Print the factor for debugging and testing (implementing Testable) */ + virtual void print(const std::string& s = "", + const IndexFormatter& formatter = DefaultKeyFormatter) const; + + /** Compare to another factor for testing (implementing Testable) */ + virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; + + /** Evaluate the factor error f(x), see above. */ + virtual double error(const VectorValues& c) const; /** 0.5*[x -1]'*H*[x -1] (also see constructor documentation) */ + + /** Return the dimension of the variable pointed to by the given key iterator + * todo: Remove this in favor of keeping track of dimensions with variables? + * @param variable An iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + */ + virtual size_t getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); } + + /** Return the number of columns and rows of the Hessian matrix */ + size_t rows() const { return info_.rows(); } + + /** + * Construct the corresponding anti-factor to negate information + * stored stored in this factor. + * @return a HessianFactor with negated Hessian matrices + */ + virtual GaussianFactor::shared_ptr negate() const; + + /** Check if the factor is empty. TODO: How should this be defined? */ + virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; } + + /** Return a view of the block at (j1,j2) of the upper-triangular part of the + * information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation + * above to explain that only the upper-triangular part of the information matrix is stored + * and returned by this function. + * @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return A view of the requested block, not a copy. + */ + constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); } + + /** Return a view of the block at (j1,j2) of the upper-triangular part of the + * information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation + * above to explain that only the upper-triangular part of the information matrix is stored + * and returned by this function. + * @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return A view of the requested block, not a copy. + */ + Block info(iterator j1, iterator j2) { return info_(j1-begin(), j2-begin()); } + + /** Return the upper-triangular part of the full *augmented* information matrix, + * as described above. See HessianFactor class documentation above to explain that only the + * upper-triangular part of the information matrix is stored and returned by this function. + */ + constBlock info() const { return info_.full(); } + + /** Return the upper-triangular part of the full *augmented* information matrix, + * as described above. See HessianFactor class documentation above to explain that only the + * upper-triangular part of the information matrix is stored and returned by this function. + */ + Block info() { return info_.full(); } + + /** Return the constant term \f$ f \f$ as described above + * @return The constant term \f$ f \f$ + */ + double constantTerm() const { return info_(this->size(), this->size())(0,0); } + + /** Return the constant term \f$ f \f$ as described above + * @return The constant term \f$ f \f$ + */ + double& constantTerm() { return info_(this->size(), this->size())(0,0); } + + /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. + * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return The linear term \f$ g \f$ */ + constBlock::ColXpr linearTerm(const_iterator j) const { return info_(j-begin(), size()).col(0); } + + /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. + * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return The linear term \f$ g \f$ */ + Block::ColXpr linearTerm(iterator j) { return info_(j-begin(), size()).col(0); } + + /** Return the complete linear term \f$ g \f$ as described above. + * @return The linear term \f$ g \f$ */ + constBlock::ColXpr linearTerm() const { return info_.range(0, this->size(), this->size(), this->size() + 1).col(0); } + + /** Return the complete linear term \f$ g \f$ as described above. + * @return The linear term \f$ g \f$ */ + Block::ColXpr linearTerm() { return info_.range(0, this->size(), this->size(), this->size() + 1).col(0); } + + /** Return the augmented information matrix represented by this GaussianFactor. + * The augmented information matrix contains the information matrix with an + * additional column holding the information vector, and an additional row + * holding the transpose of the information vector. The lower-right entry + * contains the constant error term (when \f$ \delta x = 0 \f$). The + * augmented information matrix is described in more detail in HessianFactor, + * which in fact stores an augmented information matrix. + * + * For HessianFactor, this is the same as info() except that this function + * returns a complete symmetric matrix whereas info() returns a matrix where + * only the upper triangle is valid, but should be interpreted as symmetric. + * This is because info() returns only a reference to the internal + * representation of the augmented information matrix, which stores only the + * upper triangle. + */ + virtual Matrix augmentedInformation() const; + + /** Return the non-augmented information matrix represented by this + * GaussianFactor. + */ + virtual Matrix information() const; + + /** + * Return (dense) matrix associated with factor + * @param ordering of variables needed for matrix column order + * @param set weight to true to bake in the weights + */ + virtual std::pair jacobian() const; + + /** + * Return (dense) matrix associated with factor + * The returned system is an augmented matrix: [A b] + * @param set weight to use whitening to bake in weights + */ + virtual Matrix augmentedJacobian() const; + + // Friend unit test classes + friend class ::HessianFactorConversionConstructorTest; + friend class ::HessianFactorConstructor1Test; + friend class ::HessianFactorConstructor1bTest; + friend class ::HessianFactorcombineTest; + + // used in eliminateCholesky: + + /** + * Do Cholesky. Note that after this, the lower triangle still contains + * some untouched non-zeros that should be zero. We zero them while + * extracting submatrices in splitEliminatedFactor. Frank says :-( + */ + void partialCholesky(size_t nrFrontals); + + /** Update the factor by adding the information from the JacobianFactor + * (used internally during elimination). + * @param update The JacobianFactor containing the new information to add + * @param scatter A mapping from variable index to slot index in this HessianFactor + */ + void updateATA(const JacobianFactor& update, const Scatter& scatter); + + /** Update the factor by adding the information from the HessianFactor + * (used internally during elimination). + * @param update The HessianFactor containing the new information to add + * @param scatter A mapping from variable index to slot index in this HessianFactor + */ + void updateATA(const HessianFactor& update, const Scatter& scatter); + + /** assert invariants */ + void assertInvariants() const; + + /** + * Densely partially eliminate with Cholesky factorization. JacobianFactors + * are left-multiplied with their transpose to form the Hessian using the + * conversion constructor HessianFactor(const JacobianFactor&). + * + * If any factors contain constrained noise models, this function will fail + * because our current implementation cannot handle constrained noise models + * in Cholesky factorization. The function EliminatePreferCholesky() + * automatically does QR instead when this is the case. + * + * Variables are eliminated in the natural order of the variable indices of in + * the factors. + * @param factors Factors to combine and eliminate + * @param nrFrontals Number of frontal variables to eliminate. + * @return The conditional and remaining factor + + * \addtogroup LinearSolving + */ + friend GTSAM_EXPORT std::pair, boost::shared_ptr > + EliminateCholeskyOrdered(const GaussianFactorGraph& factors, const Ordering& keys); + + private: + + /** split partially eliminated factor */ + boost::shared_ptr splitEliminatedFactor(size_t nrFrontals); + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor); + ar & BOOST_SERIALIZATION_NVP(info_); + } + }; + +} + diff --git a/gtsam/linear/HessianFactorOrdered.cpp b/gtsam/linear/HessianFactorOrdered.cpp index 1929e2a53..a619568c4 100644 --- a/gtsam/linear/HessianFactorOrdered.cpp +++ b/gtsam/linear/HessianFactorOrdered.cpp @@ -47,19 +47,19 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -string SlotEntry::toString() const { +string SlotEntryOrdered::toString() const { ostringstream oss; - oss << "SlotEntry: slot=" << slot << ", dim=" << dimension; + oss << "SlotEntryOrdered: slot=" << slot << ", dim=" << dimension; return oss.str(); } /* ************************************************************************* */ -Scatter::Scatter(const FactorGraphOrdered& gfg) { +ScatterOrdered::ScatterOrdered(const FactorGraphOrdered& gfg) { // First do the set union. BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, gfg) { if(factor) { for(GaussianFactorOrdered::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) { - this->insert(make_pair(*variable, SlotEntry(0, factor->getDim(variable)))); + this->insert(make_pair(*variable, SlotEntryOrdered(0, factor->getDim(variable)))); } } } @@ -270,12 +270,12 @@ HessianFactorOrdered::HessianFactorOrdered(const GaussianFactorOrdered& gf) : in /* ************************************************************************* */ HessianFactorOrdered::HessianFactorOrdered(const FactorGraphOrdered& factors) : info_(matrix_) { - Scatter scatter(factors); + ScatterOrdered scatter(factors); // Pull out keys and dimensions gttic(keys); vector dimensions(scatter.size() + 1); - BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { + BOOST_FOREACH(const ScatterOrdered::value_type& var_slot, scatter) { dimensions[var_slot.second.slot] = var_slot.second.dimension; } // This is for the r.h.s. vector @@ -288,7 +288,7 @@ HessianFactorOrdered::HessianFactorOrdered(const FactorGraphOrdered class BayesNetOrdered; - // Definition of Scatter, which is an intermediate data structure used when + // Definition of ScatterOrdered, which is an intermediate data structure used when // building a HessianFactor incrementally, to get the keys in the right // order. // The "scatter" is a map from global variable indices to slot indices in the // union of involved variables. We also include the dimensionality of the // variable. - struct GTSAM_EXPORT SlotEntry { + struct GTSAM_EXPORT SlotEntryOrdered { size_t slot; size_t dimension; - SlotEntry(size_t _slot, size_t _dimension) + SlotEntryOrdered(size_t _slot, size_t _dimension) : slot(_slot), dimension(_dimension) {} std::string toString() const; }; - class Scatter : public FastMap { + class ScatterOrdered : public FastMap { public: - Scatter() {} - Scatter(const FactorGraphOrdered& gfg); + ScatterOrdered() {} + ScatterOrdered(const FactorGraphOrdered& gfg); }; /** @@ -350,14 +350,14 @@ namespace gtsam { * @param update The JacobianFactor containing the new information to add * @param scatter A mapping from variable index to slot index in this HessianFactor */ - void updateATA(const JacobianFactorOrdered& update, const Scatter& scatter); + void updateATA(const JacobianFactorOrdered& update, const ScatterOrdered& scatter); /** Update the factor by adding the information from the HessianFactor * (used internally during elimination). * @param update The HessianFactor containing the new information to add * @param scatter A mapping from variable index to slot index in this HessianFactor */ - void updateATA(const HessianFactorOrdered& update, const Scatter& scatter); + void updateATA(const HessianFactorOrdered& update, const ScatterOrdered& scatter); /** assert invariants */ void assertInvariants() const; diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index ef75cd884..274f0b905 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -65,7 +65,7 @@ namespace gtsam { /* ************************************************************************* */ namespace { - DenseIndex _getCols(const std::pair& p) { + DenseIndex _getColsJF(const std::pair& p) { return p.second.cols(); } } @@ -86,9 +86,9 @@ namespace gtsam { // a single '1' to add a dimension for the b vector. { using boost::adaptors::transformed; - using boost::join; using boost::assign::cref_list_of; - Ab_ = VerticalBlockMatrix(join(terms | transformed(_getCols), cref_list_of<1,DenseIndex>(1)), b.size()); + namespace br = boost::range; + Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&_getColsJF), cref_list_of<1,DenseIndex>(1)), b.size()); } // Check and add terms diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index aa6c4609d..32968d8e0 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -20,7 +20,7 @@ #include #include #include -//#include +#include #include #include #include @@ -60,15 +60,17 @@ namespace gtsam { /* ************************************************************************* */ JacobianFactor::JacobianFactor() : Ab_(cref_list_of<1>(1), 0) - {} + { + getb().setZero(); + } /* ************************************************************************* */ JacobianFactor::JacobianFactor(const GaussianFactor& gf) { // Copy the matrix data depending on what type of factor we're copying from if(const JacobianFactor* rhs = dynamic_cast(&gf)) *this = JacobianFactor(*rhs); - //else if(const HessianFactor* rhs = dynamic_cast(&gf)) - // *this = JacobianFactor(*rhs); + else if(const HessianFactor* rhs = dynamic_cast(&gf)) + *this = JacobianFactor(*rhs); else throw std::invalid_argument("In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor"); } @@ -441,24 +443,33 @@ namespace gtsam { } /* ************************************************************************* */ - pair JacobianFactor::jacobian(bool weight) const { + pair JacobianFactor::jacobian() const { + pair result = jacobianUnweighted(); + // divide in sigma so error is indeed 0.5*|Ax-b| + if (model_) + model_->WhitenSystem(result.first, result.second); + return result; + } + + /* ************************************************************************* */ + pair JacobianFactor::jacobianUnweighted() const { Matrix A(Ab_.range(0, size())); Vector b(getb()); - // divide in sigma so error is indeed 0.5*|Ax-b| - if (weight && model_) - model_->WhitenSystem(A,b); return make_pair(A, b); } /* ************************************************************************* */ - Matrix JacobianFactor::augmentedJacobian(bool weight) const { - if (weight && model_) { - Matrix Ab(Ab_.range(0,Ab_.nBlocks())); + Matrix JacobianFactor::augmentedJacobian() const { + Matrix Ab = augmentedJacobianUnweighted(); + if (model_) { model_->WhitenInPlace(Ab); - return Ab; - } else { - return Ab_.range(0, Ab_.nBlocks()); } + return Ab; + } + + /* ************************************************************************* */ + Matrix JacobianFactor::augmentedJacobianUnweighted() const { + return Ab_.range(0, Ab_.nBlocks()); } /* ************************************************************************* */ @@ -472,10 +483,10 @@ namespace gtsam { } /* ************************************************************************* */ - //GaussianFactor::shared_ptr JacobianFactor::negate() const { - // HessianFactor hessian(*this); - // return hessian.negate(); - //} + GaussianFactor::shared_ptr JacobianFactor::negate() const { + HessianFactor hessian(*this); + return hessian.negate(); + } /* ************************************************************************* */ std::pair, boost::shared_ptr > diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 960c8e976..cadcfe093 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -88,8 +88,8 @@ namespace gtsam { public: typedef VerticalBlockMatrix::Block ABlock; typedef VerticalBlockMatrix::constBlock constABlock; - typedef VerticalBlockMatrix::Column BVector; - typedef VerticalBlockMatrix::constColumn constBVector; + typedef ABlock::ColXpr BVector; + typedef constABlock::ConstColXpr constBVector; /** Convert from other GaussianFactor */ explicit JacobianFactor(const GaussianFactor& gf); @@ -178,26 +178,37 @@ namespace gtsam { * @param ordering of variables needed for matrix column order * @param set weight to true to bake in the weights */ - virtual std::pair jacobian(bool weight = true) const; + virtual std::pair jacobian() const; + + /** + * Return (dense) matrix associated with factor + * @param ordering of variables needed for matrix column order + * @param set weight to true to bake in the weights + */ + std::pair jacobianUnweighted() const; /** * Return (dense) matrix associated with factor * The returned system is an augmented matrix: [A b] * @param set weight to use whitening to bake in weights */ - virtual Matrix augmentedJacobian(bool weight = true) const; + virtual Matrix augmentedJacobian() const; + + /** + * Return (dense) matrix associated with factor + * The returned system is an augmented matrix: [A b] + * @param set weight to use whitening to bake in weights + */ + Matrix augmentedJacobianUnweighted() const; /** * Construct the corresponding anti-factor to negate information * stored stored in this factor. * @return a HessianFactor with negated Hessian matrices */ - //virtual GaussianFactor::shared_ptr negate() const; + virtual GaussianFactor::shared_ptr negate() const; - /** Check if the factor contains no information, i.e. zero rows. This does - * not necessarily mean that the factor involves no variables (to check for - * involving no variables use keys().empty()). - */ + /** Check if the factor is empty. TODO: How should this be defined? */ virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; } /** is noise model constrained ? */ @@ -284,7 +295,7 @@ namespace gtsam { * factor to be the remaining component. Performs same operation as eliminate(), * but without running QR. */ - boost::shared_ptr splitConditional(size_t nrFrontals = 1); + boost::shared_ptr splitConditional(size_t nrFrontals); /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index fbded1a9f..17adf5018 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -250,8 +250,8 @@ namespace gtsam { VectorValues operator*(const double a, const VectorValues &v) { VectorValues result; - BOOST_FOREACH(const VectorValues::KeyValuePair& v, v) - result.values_.insert(result.values_.end(), make_pair(v.first, a * v.second)); + BOOST_FOREACH(const VectorValues::KeyValuePair& key_v, v) + result.values_.insert(result.values_.end(), make_pair(key_v.first, a * key_v.second)); return result; } diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 0014f0e69..a3c5d3f2c 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -197,11 +197,11 @@ namespace gtsam { iterator begin() { return values_.begin(); } ///< Iterator over variables const_iterator begin() const { return values_.begin(); } ///< Iterator over variables - iterator end() { return values_.end(); } ///< Iterator over variables + iterator end() { return values_.end(); } ///< Iterator over variables const_iterator end() const { return values_.end(); } ///< Iterator over variables - reverse_iterator rbegin() { return values_.rbegin(); } ///< Reverse iterator over variables + reverse_iterator rbegin() { return values_.rbegin(); } ///< Reverse iterator over variables const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables - reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables + reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */ diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactorObsolete.cpp similarity index 100% rename from gtsam/linear/tests/testHessianFactor.cpp rename to gtsam/linear/tests/testHessianFactorObsolete.cpp diff --git a/gtsam/linear/tests/testHessianFactorUnordered.cpp b/gtsam/linear/tests/testHessianFactorUnordered.cpp new file mode 100644 index 000000000..f038624c6 --- /dev/null +++ b/gtsam/linear/tests/testHessianFactorUnordered.cpp @@ -0,0 +1,638 @@ +/* ---------------------------------------------------------------------------- + + * 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 testCholeskyFactor.cpp + * @author Richard Roberts + * @date Dec 15, 2010 + */ + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace boost::assign; +using namespace gtsam; + +const double tol = 1e-5; + +#if 0 +/* ************************************************************************* */ +TEST(HessianFactor, emptyConstructor) { + HessianFactor f; + DOUBLES_EQUAL(0.0, f.constantTerm(), 1e-9); // Constant term should be zero + EXPECT(assert_equal(Vector(), f.linearTerm())); // Linear term should be empty + EXPECT(assert_equal(zeros(1,1), f.info())); // Full matrix should be 1-by-1 zero matrix + DOUBLES_EQUAL(0.0, f.error(VectorValuesOrdered()), 1e-9); // Should have zero error +} + +/* ************************************************************************* */ +TEST(HessianFactor, ConversionConstructor) { + + HessianFactorOrdered expected; + expected.keys_.push_back(0); + expected.keys_.push_back(1); + size_t dims[] = { 2, 4, 1 }; + expected.info_.resize(dims, dims+3, false); + expected.matrix_ = Matrix_(7,7, + 125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000, + 0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000, + -25.0000, 0.0, 25.0000, 0.0, 0.0, 0.0, -5.0000, + 0.0, -25.0000, 0.0, 25.0000, 0.0, 0.0, 7.5000, + -100.0000, 0.0, 0.0, 0.0, 100.0000, 0.0, -20.0000, + 0.0, -100.0000, 0.0, 0.0, 0.0, 100.0000, 10.0000, + 25.0000, -17.5000, -5.0000, 7.5000, -20.0000, 10.0000, 8.2500); + + // sigmas + double sigma1 = 0.2; + double sigma2 = 0.1; + Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2); + + // the combined linear factor + Matrix Ax2 = Matrix_(4,2, + // x2 + -1., 0., + +0.,-1., + 1., 0., + +0.,1. + ); + + Matrix Al1x1 = Matrix_(4,4, + // l1 x1 + 1., 0., 0.00, 0., // f4 + 0., 1., 0.00, 0., // f4 + 0., 0., -1., 0., // f2 + 0., 0., 0.00,-1. // f2 + ); + + // the RHS + Vector b2(4); + b2(0) = -0.2; + b2(1) = 0.3; + b2(2) = 0.2; + b2(3) = -0.1; + + vector > meas; + meas.push_back(make_pair(0, Ax2)); + meas.push_back(make_pair(1, Al1x1)); + JacobianFactorOrdered combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas)); + + HessianFactorOrdered actual(combined); + + VectorValuesOrdered values(std::vector(dims, dims+2)); + values[0] = Vector_(2, 1.0, 2.0); + values[1] = Vector_(4, 3.0, 4.0, 5.0, 6.0); + + EXPECT_LONGS_EQUAL(2, actual.size()); + + EXPECT(assert_equal(expected, actual, 1e-9)); + + // error terms + EXPECT_DOUBLES_EQUAL(combined.error(values), actual.error(values), 1e-9); +} + +/* ************************************************************************* */ +TEST(HessianFactorOrdered, Constructor1) +{ + Matrix G = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); + Vector g = Vector_(2, -8.0, -9.0); + double f = 10.0; + + Vector dxv = Vector_(2, 1.5, 2.5); + + vector dims; + dims.push_back(2); + VectorValuesOrdered dx(dims); + + dx[0] = dxv; + + HessianFactorOrdered factor(0, G, g, f); + + // extract underlying parts + Matrix info_matrix = factor.info_.range(0, 1, 0, 1); + EXPECT(assert_equal(Matrix(G), info_matrix)); + EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10); + EXPECT(assert_equal(g, Vector(factor.linearTerm()), 1e-10)); + EXPECT_LONGS_EQUAL(1, factor.size()); + + // error 0.5*(f - 2*x'*g + x'*G*x) + double expected = 80.375; + double actual = factor.error(dx); + double expected_manual = 0.5 * (f - 2.0 * dxv.dot(g) + dxv.transpose() * G.selfadjointView() * dxv); + EXPECT_DOUBLES_EQUAL(expected, expected_manual, 1e-10); + DOUBLES_EQUAL(expected, actual, 1e-10); +} + +/* ************************************************************************* */ +TEST(HessianFactorOrdered, Constructor1b) +{ + Vector mu = Vector_(2,1.0,2.0); + Matrix Sigma = eye(2,2); + + HessianFactorOrdered factor(0, mu, Sigma); + + Matrix G = eye(2,2); + Vector g = G*mu; + double f = dot(g,mu); + + // Check + Matrix info_matrix = factor.info_.range(0, 1, 0, 1); + EXPECT(assert_equal(Matrix(G), info_matrix)); + EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10); + EXPECT(assert_equal(g, Vector(factor.linearTerm()), 1e-10)); + EXPECT_LONGS_EQUAL(1, factor.size()); +} + +/* ************************************************************************* */ +TEST(HessianFactorOrdered, Constructor2) +{ + Matrix G11 = Matrix_(1,1, 1.0); + Matrix G12 = Matrix_(1,2, 2.0, 4.0); + Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); + Vector g1 = Vector_(1, -7.0); + Vector g2 = Vector_(2, -8.0, -9.0); + double f = 10.0; + + Vector dx0 = Vector_(1, 0.5); + Vector dx1 = Vector_(2, 1.5, 2.5); + + vector dims; + dims.push_back(1); + dims.push_back(2); + VectorValuesOrdered dx(dims); + + dx[0] = dx0; + dx[1] = dx1; + + HessianFactorOrdered factor(0, 1, G11, G12, g1, G22, g2, f); + + double expected = 90.5; + double actual = factor.error(dx); + + DOUBLES_EQUAL(expected, actual, 1e-10); + LONGS_EQUAL(4, factor.rows()); + DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10); + + Vector linearExpected(3); linearExpected << g1, g2; + EXPECT(assert_equal(linearExpected, factor.linearTerm())); + + EXPECT(assert_equal(G11, factor.info(factor.begin(), factor.begin()))); + EXPECT(assert_equal(G12, factor.info(factor.begin(), factor.begin()+1))); + EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1))); + + // Check case when vector values is larger than factor + dims.push_back(2); + VectorValuesOrdered dxLarge(dims); + dxLarge[0] = dx0; + dxLarge[1] = dx1; + dxLarge[2] = Vector_(2, 0.1, 0.2); + EXPECT_DOUBLES_EQUAL(expected, factor.error(dxLarge), 1e-10); +} + +/* ************************************************************************* */ +TEST(HessianFactorOrdered, Constructor3) +{ + Matrix G11 = Matrix_(1,1, 1.0); + Matrix G12 = Matrix_(1,2, 2.0, 4.0); + Matrix G13 = Matrix_(1,3, 3.0, 6.0, 9.0); + + Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); + Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); + + Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0); + + Vector g1 = Vector_(1, -7.0); + Vector g2 = Vector_(2, -8.0, -9.0); + Vector g3 = Vector_(3, 1.0, 2.0, 3.0); + + double f = 10.0; + + Vector dx0 = Vector_(1, 0.5); + Vector dx1 = Vector_(2, 1.5, 2.5); + Vector dx2 = Vector_(3, 1.5, 2.5, 3.5); + + vector dims; + dims.push_back(1); + dims.push_back(2); + dims.push_back(3); + VectorValuesOrdered dx(dims); + + dx[0] = dx0; + dx[1] = dx1; + dx[2] = dx2; + + HessianFactorOrdered factor(0, 1, 2, G11, G12, G13, g1, G22, G23, g2, G33, g3, f); + + double expected = 371.3750; + double actual = factor.error(dx); + + DOUBLES_EQUAL(expected, actual, 1e-10); + LONGS_EQUAL(7, factor.rows()); + DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10); + + Vector linearExpected(6); linearExpected << g1, g2, g3; + EXPECT(assert_equal(linearExpected, factor.linearTerm())); + + EXPECT(assert_equal(G11, factor.info(factor.begin()+0, factor.begin()+0))); + EXPECT(assert_equal(G12, factor.info(factor.begin()+0, factor.begin()+1))); + EXPECT(assert_equal(G13, factor.info(factor.begin()+0, factor.begin()+2))); + EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1))); + EXPECT(assert_equal(G23, factor.info(factor.begin()+1, factor.begin()+2))); + EXPECT(assert_equal(G33, factor.info(factor.begin()+2, factor.begin()+2))); +} + +/* ************************************************************************* */ +TEST(HessianFactorOrdered, ConstructorNWay) +{ + Matrix G11 = Matrix_(1,1, 1.0); + Matrix G12 = Matrix_(1,2, 2.0, 4.0); + Matrix G13 = Matrix_(1,3, 3.0, 6.0, 9.0); + + Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); + Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); + + Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0); + + Vector g1 = Vector_(1, -7.0); + Vector g2 = Vector_(2, -8.0, -9.0); + Vector g3 = Vector_(3, 1.0, 2.0, 3.0); + + double f = 10.0; + + Vector dx0 = Vector_(1, 0.5); + Vector dx1 = Vector_(2, 1.5, 2.5); + Vector dx2 = Vector_(3, 1.5, 2.5, 3.5); + + vector dims; + dims.push_back(1); + dims.push_back(2); + dims.push_back(3); + VectorValuesOrdered dx(dims); + + dx[0] = dx0; + dx[1] = dx1; + dx[2] = dx2; + + + std::vector js; + js.push_back(0); js.push_back(1); js.push_back(2); + 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); + HessianFactorOrdered factor(js, Gs, gs, f); + + double expected = 371.3750; + double actual = factor.error(dx); + + DOUBLES_EQUAL(expected, actual, 1e-10); + LONGS_EQUAL(7, factor.rows()); + DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10); + + Vector linearExpected(6); linearExpected << g1, g2, g3; + EXPECT(assert_equal(linearExpected, factor.linearTerm())); + + EXPECT(assert_equal(G11, factor.info(factor.begin()+0, factor.begin()+0))); + EXPECT(assert_equal(G12, factor.info(factor.begin()+0, factor.begin()+1))); + EXPECT(assert_equal(G13, factor.info(factor.begin()+0, factor.begin()+2))); + EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1))); + EXPECT(assert_equal(G23, factor.info(factor.begin()+1, factor.begin()+2))); + EXPECT(assert_equal(G33, factor.info(factor.begin()+2, factor.begin()+2))); +} + +/* ************************************************************************* */ +TEST_UNSAFE(HessianFactorOrdered, CopyConstructor_and_assignment) +{ + Matrix G11 = Matrix_(1,1, 1.0); + Matrix G12 = Matrix_(1,2, 2.0, 4.0); + Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); + Vector g1 = Vector_(1, -7.0); + Vector g2 = Vector_(2, -8.0, -9.0); + double f = 10.0; + + Vector dx0 = Vector_(1, 0.5); + Vector dx1 = Vector_(2, 1.5, 2.5); + + vector dims; + dims.push_back(1); + dims.push_back(2); + VectorValuesOrdered dx(dims); + + dx[0] = dx0; + dx[1] = dx1; + + HessianFactorOrdered originalFactor(0, 1, G11, G12, g1, G22, g2, f); + + // Make a copy + HessianFactorOrdered copy1(originalFactor); + + double expected = 90.5; + double actual = copy1.error(dx); + + DOUBLES_EQUAL(expected, actual, 1e-10); + LONGS_EQUAL(4, copy1.rows()); + DOUBLES_EQUAL(10.0, copy1.constantTerm(), 1e-10); + + Vector linearExpected(3); linearExpected << g1, g2; + EXPECT(assert_equal(linearExpected, copy1.linearTerm())); + + EXPECT(assert_equal(G11, copy1.info(copy1.begin(), copy1.begin()))); + EXPECT(assert_equal(G12, copy1.info(copy1.begin(), copy1.begin()+1))); + EXPECT(assert_equal(G22, copy1.info(copy1.begin()+1, copy1.begin()+1))); + + // Make a copy using the assignment operator + HessianFactorOrdered copy2; + copy2 = HessianFactorOrdered(originalFactor); // Make a temporary to make sure copying does not shared references + + actual = copy2.error(dx); + DOUBLES_EQUAL(expected, actual, 1e-10); + LONGS_EQUAL(4, copy2.rows()); + DOUBLES_EQUAL(10.0, copy2.constantTerm(), 1e-10); + + EXPECT(assert_equal(linearExpected, copy2.linearTerm())); + + EXPECT(assert_equal(G11, copy2.info(copy2.begin(), copy2.begin()))); + EXPECT(assert_equal(G12, copy2.info(copy2.begin(), copy2.begin()+1))); + EXPECT(assert_equal(G22, copy2.info(copy2.begin()+1, copy2.begin()+1))); +} + +/* ************************************************************************* */ +TEST_UNSAFE(HessianFactorOrdered, CombineAndEliminate) +{ + Matrix A01 = Matrix_(3,3, + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0); + Vector b0 = Vector_(3, 1.5, 1.5, 1.5); + Vector s0 = Vector_(3, 1.6, 1.6, 1.6); + + Matrix A10 = Matrix_(3,3, + 2.0, 0.0, 0.0, + 0.0, 2.0, 0.0, + 0.0, 0.0, 2.0); + Matrix A11 = Matrix_(3,3, + -2.0, 0.0, 0.0, + 0.0, -2.0, 0.0, + 0.0, 0.0, -2.0); + Vector b1 = Vector_(3, 2.5, 2.5, 2.5); + Vector s1 = Vector_(3, 2.6, 2.6, 2.6); + + Matrix A21 = Matrix_(3,3, + 3.0, 0.0, 0.0, + 0.0, 3.0, 0.0, + 0.0, 0.0, 3.0); + Vector b2 = Vector_(3, 3.5, 3.5, 3.5); + Vector s2 = Vector_(3, 3.6, 3.6, 3.6); + + GaussianFactorGraphOrdered gfg; + gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); + gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); + gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); + + Matrix zero3x3 = zeros(3,3); + Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); + Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); + Vector b = gtsam::concatVectors(3, &b1, &b0, &b2); + Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2); + + // create a full, uneliminated version of the factor + JacobianFactorOrdered expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); + + // perform elimination + GaussianConditionalOrdered::shared_ptr expectedBN = expectedFactor.eliminate(1); + + // create expected Hessian after elimination + HessianFactorOrdered expectedCholeskyFactor(expectedFactor); + + // Convert all factors to hessians + FactorGraphOrdered hessians; + BOOST_FOREACH(const GaussianFactorGraphOrdered::sharedFactor& factor, gfg) { + if(boost::shared_ptr hf = boost::dynamic_pointer_cast(factor)) + hessians.push_back(hf); + else if(boost::shared_ptr jf = boost::dynamic_pointer_cast(factor)) + hessians.push_back(boost::make_shared(*jf)); + else + CHECK(false); + } + + // Eliminate + GaussianFactorGraphOrdered::EliminationResult actualCholesky = EliminateCholeskyOrdered(gfg, 1); + HessianFactorOrdered::shared_ptr actualFactor = boost::dynamic_pointer_cast(actualCholesky.second); + + EXPECT(assert_equal(*expectedBN, *actualCholesky.first, 1e-6)); + EXPECT(assert_equal(expectedCholeskyFactor, *actualFactor, 1e-6)); +} + +/* ************************************************************************* */ +TEST(HessianFactorOrdered, eliminate2 ) +{ + // sigmas + double sigma1 = 0.2; + double sigma2 = 0.1; + Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2); + + // the combined linear factor + Matrix Ax2 = Matrix_(4,2, + // x2 + -1., 0., + +0.,-1., + 1., 0., + +0.,1. + ); + + Matrix Al1x1 = Matrix_(4,4, + // l1 x1 + 1., 0., 0.00, 0., // f4 + 0., 1., 0.00, 0., // f4 + 0., 0., -1., 0., // f2 + 0., 0., 0.00,-1. // f2 + ); + + // the RHS + Vector b2(4); + b2(0) = -0.2; + b2(1) = 0.3; + b2(2) = 0.2; + b2(3) = -0.1; + + vector > meas; + meas.push_back(make_pair(0, Ax2)); + meas.push_back(make_pair(1, Al1x1)); + JacobianFactorOrdered combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas)); + + // eliminate the combined factor + HessianFactorOrdered::shared_ptr combinedLF_Chol(new HessianFactorOrdered(combined)); + FactorGraphOrdered combinedLFG_Chol; + combinedLFG_Chol.push_back(combinedLF_Chol); + + GaussianFactorGraphOrdered::EliminationResult actual_Chol = EliminateCholeskyOrdered( + combinedLFG_Chol, 1); + HessianFactorOrdered::shared_ptr actualFactor = boost::dynamic_pointer_cast< + HessianFactorOrdered>(actual_Chol.second); + + // create expected Conditional Gaussian + double oldSigma = 0.0894427; // from when R was made unit + Matrix R11 = Matrix_(2,2, + 1.00, 0.00, + 0.00, 1.00 + )/oldSigma; + Matrix S12 = Matrix_(2,4, + -0.20, 0.00,-0.80, 0.00, + +0.00,-0.20,+0.00,-0.80 + )/oldSigma; + Vector d = Vector_(2,0.2,-0.14)/oldSigma; + GaussianConditionalOrdered expectedCG(0,d,R11,1,S12,ones(2)); + EXPECT(assert_equal(expectedCG,*actual_Chol.first,1e-4)); + + // the expected linear factor + double sigma = 0.2236; + Matrix Bl1x1 = Matrix_(2,4, + // l1 x1 + 1.00, 0.00, -1.00, 0.00, + 0.00, 1.00, +0.00, -1.00 + )/sigma; + Vector b1 = Vector_(2,0.0,0.894427); + JacobianFactorOrdered expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0)); + EXPECT(assert_equal(HessianFactorOrdered(expectedLF), *actualFactor, 1.5e-3)); +} + +/* ************************************************************************* */ +TEST(HessianFactorOrdered, eliminateUnsorted) { + + JacobianFactorOrdered::shared_ptr factor1( + new JacobianFactorOrdered(0, + Matrix_(3,3, + 44.7214, 0.0, 0.0, + 0.0, 44.7214, 0.0, + 0.0, 0.0, 44.7214), + 1, + Matrix_(3,3, + -0.179168, -44.721, 0.717294, + 44.721, -0.179168, -44.9138, + 0.0, 0.0, -44.7214), + Vector_(3, 1.98916e-17, -4.96503e-15, -7.75792e-17), + noiseModel::Unit::Create(3))); + HessianFactorOrdered::shared_ptr unsorted_factor2( + new HessianFactorOrdered(JacobianFactorOrdered(0, + Matrix_(6,3, + 25.8367, 0.1211, 0.0593, + 0.0, 23.4099, 30.8733, + 0.0, 0.0, 25.8729, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0), + 1, + Matrix_(6,3, + 25.7429, -1.6897, 0.4587, + 1.6400, 23.3095, -8.4816, + 0.0034, 0.0509, -25.7855, + 0.9997, -0.0002, 0.0824, + 0.0, 0.9973, 0.9517, + 0.0, 0.0, 0.9973), + Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), + noiseModel::Unit::Create(6)))); + Permutation permutation(2); + permutation[0] = 1; + permutation[1] = 0; + unsorted_factor2->permuteWithInverse(permutation); + + HessianFactorOrdered::shared_ptr sorted_factor2( + new HessianFactorOrdered(JacobianFactorOrdered(0, + Matrix_(6,3, + 25.7429, -1.6897, 0.4587, + 1.6400, 23.3095, -8.4816, + 0.0034, 0.0509, -25.7855, + 0.9997, -0.0002, 0.0824, + 0.0, 0.9973, 0.9517, + 0.0, 0.0, 0.9973), + 1, + Matrix_(6,3, + 25.8367, 0.1211, 0.0593, + 0.0, 23.4099, 30.8733, + 0.0, 0.0, 25.8729, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0), + Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), + noiseModel::Unit::Create(6)))); + + GaussianFactorGraphOrdered sortedGraph; +// sortedGraph.push_back(factor1); + sortedGraph.push_back(sorted_factor2); + + GaussianFactorGraphOrdered unsortedGraph; +// unsortedGraph.push_back(factor1); + unsortedGraph.push_back(unsorted_factor2); + + GaussianConditionalOrdered::shared_ptr expected_bn; + GaussianFactorOrdered::shared_ptr expected_factor; + boost::tie(expected_bn, expected_factor) = + EliminatePreferCholeskyOrdered(sortedGraph, 1); + + GaussianConditionalOrdered::shared_ptr actual_bn; + GaussianFactorOrdered::shared_ptr actual_factor; + boost::tie(actual_bn, actual_factor) = + EliminatePreferCholeskyOrdered(unsortedGraph, 1); + + EXPECT(assert_equal(*expected_bn, *actual_bn, 1e-10)); + EXPECT(assert_equal(*expected_factor, *actual_factor, 1e-10)); +} + +/* ************************************************************************* */ +TEST(HessianFactorOrdered, combine) { + + // update the information matrix with a single jacobian factor + Matrix A0 = Matrix_(2, 2, + 11.1803399, 0.0, + 0.0, 11.1803399); + Matrix A1 = Matrix_(2, 2, + -2.23606798, 0.0, + 0.0, -2.23606798); + Matrix A2 = Matrix_(2, 2, + -8.94427191, 0.0, + 0.0, -8.94427191); + Vector b = Vector_(2, 2.23606798,-1.56524758); + SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2)); + GaussianFactorOrdered::shared_ptr f(new JacobianFactorOrdered(0, A0, 1, A1, 2, A2, b, model)); + FactorGraphOrdered factors; + factors.push_back(f); + + // Form Ab' * Ab + HessianFactorOrdered actual(factors); + + Matrix expected = Matrix_(7, 7, + 125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000, + 0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000, + -25.0000, 0.0, 5.0000, 0.0, 20.0000, 0.0, -5.0000, + 0.0, -25.0000, 0.0, 5.0000, 0.0, 20.0000, 3.5000, + -100.0000, 0.0, 20.0000, 0.0, 80.0000, 0.0, -20.0000, + 0.0, -100.0000, 0.0, 20.0000, 0.0, 80.0000, 14.0000, + 25.0000, -17.5000, -5.0000, 3.5000, -20.0000, 14.0000, 7.4500); + EXPECT(assert_equal(Matrix(expected.triangularView()), + Matrix(actual.matrix_.triangularView()), tol)); + +} +#endif + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index 1e76886bd..5e9e6ffae 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -16,6 +16,8 @@ * @date Feb 26, 2012 */ +#if 0 + #include #include #include @@ -95,3 +97,5 @@ DoglegParams DoglegOptimizer::ensureHasOrdering(DoglegParams params, const Nonli } } /* namespace gtsam */ + +#endif diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h index 2e9dd81c3..8338ec861 100644 --- a/gtsam/nonlinear/DoglegOptimizer.h +++ b/gtsam/nonlinear/DoglegOptimizer.h @@ -18,6 +18,8 @@ #pragma once +#if 0 + #include namespace gtsam { @@ -162,3 +164,5 @@ protected: }; } + +#endif diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.h b/gtsam/nonlinear/GaussNewtonOptimizer.h index 0c1da8a58..1d267071c 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.h +++ b/gtsam/nonlinear/GaussNewtonOptimizer.h @@ -61,7 +61,7 @@ public: */ GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const GaussNewtonParams& params = GaussNewtonParams()) : - NonlinearOptimizer(graph), params_(ensureHasOrdering(params, graph, initialValues)), state_(graph, initialValues) {} + NonlinearOptimizer(graph), params_(ensureHasOrdering(params, graph)), state_(graph, initialValues) {} /** Standard constructor, requires a nonlinear factor graph, initial * variable assignments, and optimization parameters. For convenience this diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 25210063b..5c860e037 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -16,6 +16,8 @@ * @author Richard Roberts */ +#if 0 + #include #include // for selective linearization thresholds #include @@ -493,3 +495,5 @@ size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThresho } } + +#endif diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 406b22374..dcd85148e 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -17,6 +17,8 @@ #pragma once +#if 0 + #include #include @@ -154,3 +156,5 @@ struct GTSAM_EXPORT ISAM2::Impl { }; } + +#endif diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 4ea251ed0..fcd1d1f4f 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -15,6 +15,8 @@ * @author Michael Kaess, Richard Roberts */ +#if 0 + #include #include // for operator += using namespace boost::assign; @@ -1150,3 +1152,5 @@ void gradientAtZero(const ISAM2& bayesTree, VectorValuesOrdered& g) { } /// namespace gtsam + +#endif diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 481dd3ff6..1e4a8bca6 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -19,6 +19,8 @@ #pragma once +#if 0 + #include #include #include @@ -729,3 +731,5 @@ GTSAM_EXPORT void gradientAtZero(const ISAM2& bayesTree, VectorValuesOrdered& g) #include #include + +#endif \ No newline at end of file diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 1b1f2e93e..a7170b22b 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -16,6 +16,8 @@ * @date Feb 26, 2012 */ +#if 0 + #include #include @@ -179,3 +181,6 @@ LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering( } } /* namespace gtsam */ + + +#endif diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index ba6f994dd..021a01159 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -18,6 +18,8 @@ #pragma once +#if 0 + #include namespace gtsam { @@ -112,7 +114,7 @@ public: */ LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const LevenbergMarquardtParams& params = LevenbergMarquardtParams()) : - NonlinearOptimizer(graph), params_(ensureHasOrdering(params, graph, initialValues)), + NonlinearOptimizer(graph), params_(ensureHasOrdering(params, graph)), state_(graph, initialValues, params_), dimensions_(initialValues.dims(*params_.ordering)) {} /** Standard constructor, requires a nonlinear factor graph, initial @@ -122,7 +124,7 @@ public: * @param graph The nonlinear factor graph to optimize * @param initialValues The initial variable assignments */ - LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const OrderingOrdered& ordering) : + LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Ordering& ordering) : NonlinearOptimizer(graph), dimensions_(initialValues.dims(ordering)) { params_.ordering = ordering; state_ = LevenbergMarquardtState(graph, initialValues, params_); } @@ -176,3 +178,5 @@ protected: }; } + +#endif diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index 8c511277b..176373ecb 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -7,10 +7,13 @@ #include #include +#include #include #include +#if 0 + namespace gtsam { /* ************************************************************************* */ @@ -55,18 +58,6 @@ LinearContainerFactor::LinearContainerFactor( initializeLinearizationPoint(linearizationPoint); } -/* ************************************************************************* */ -LinearContainerFactor::LinearContainerFactor( - const GaussianFactorOrdered::shared_ptr& factor, - const Values& linearizationPoint) -: factor_(factor->clone()) -{ - // Extract keys stashed in linear factor - BOOST_FOREACH(const Index& idx, factor_->keys()) - keys_.push_back(idx); - initializeLinearizationPoint(linearizationPoint); -} - /* ************************************************************************* */ void LinearContainerFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { Base::print(s+"LinearContainerFactor", keyFormatter); @@ -137,7 +128,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) con HessianFactor::shared_ptr hesFactor = boost::dynamic_pointer_cast(linFactor); size_t dim = hesFactor->linearTerm().size(); Eigen::Block Gview = hesFactor->info().block(0, 0, dim, dim); - Vector deltaVector = delta.asVector(); + Vector deltaVector = delta.vector(); Vector G_delta = Gview.selfadjointView() * deltaVector; hesFactor->constantTerm() += deltaVector.dot(G_delta) - 2.0 * deltaVector.dot(hesFactor->linearTerm()); hesFactor->linearTerm() -= G_delta; @@ -192,3 +183,4 @@ NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( /* ************************************************************************* */ } // \namespace gtsam +#endif diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 25020a378..304201ff8 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -11,9 +11,12 @@ #include +#if 0 + namespace gtsam { // Forward declarations + class JacobianFactor; class HessianFactor; /** @@ -45,9 +48,6 @@ public: /** Constructor from shared_ptr */ LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint = Values()); - /** Constructor from re-keyed factor: all indices assumed replaced with Key */ - LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint = Values()); - // Access const GaussianFactor::shared_ptr& factor() const { return factor_; } @@ -129,10 +129,10 @@ public: bool isHessian() const; /** Casts to JacobianFactor */ - JacobianFactor::shared_ptr toJacobian() const; + boost::shared_ptr toJacobian() const; /** Casts to HessianFactor */ - HessianFactor::shared_ptr toHessian() const; + boost::shared_ptr toHessian() const; /** * Utility function for converting linear graphs to nonlinear graphs @@ -160,4 +160,4 @@ private: } // \namespace gtsam - +#endif diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 64e450dec..d4a70f324 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -16,6 +16,8 @@ * @date May 14, 2012 */ +#if 0 + #include #include #include @@ -175,3 +177,5 @@ void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) c } } /* namespace gtsam */ + +#endif diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 0a60cf7f9..ccce60c33 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -18,6 +18,8 @@ #pragma once +#if 0 + #include #include #include @@ -141,3 +143,5 @@ protected: }; } /* namespace gtsam */ + +#endif diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index 9a34f1b6d..83ca78efb 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp @@ -5,6 +5,8 @@ * @date Jun 11, 2012 */ +#if 0 + #include #include #include @@ -62,3 +64,5 @@ const Values& NonlinearConjugateGradientOptimizer::optimize() { } } /* namespace gtsam */ + +#endif diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 767d6b999..5de4c047f 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -7,6 +7,8 @@ #pragma once +#if 0 + #include #include #include @@ -185,9 +187,6 @@ boost::tuple nonlinearConjugateGradient(const S &system, const V &ini return boost::tie(currentValues, iteration); } - - - - - } + +#endif \ No newline at end of file diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index ff3fe1649..74cb75649 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -305,7 +305,7 @@ public: this->noiseModel_->WhitenSystem(A,b); - std::vector > terms(this->size()); + std::vector > terms(this->size()); // Fill in terms for(size_t j=0; jsize(); ++j) { terms[j].first = this->keys()[j]; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index bbb58b866..2eae9b0a9 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -62,7 +62,7 @@ namespace gtsam { * tangent vector space at the linearization point. Because the tangent space is a true * vector space, the config type will be an VectorValues in that linearized factor graph. */ - class NonlinearFactorGraph: public FactorGraph { + class GTSAM_EXPORT NonlinearFactorGraph: public FactorGraph { public: @@ -86,26 +86,26 @@ namespace gtsam { NonlinearFactorGraph(const FactorGraph& graph) : Base(graph) {} /** print just calls base class */ - GTSAM_EXPORT void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** Write the graph in GraphViz format for visualization */ - GTSAM_EXPORT void saveGraph(std::ostream& stm, const Values& values = Values(), + void saveGraph(std::ostream& stm, const Values& values = Values(), const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(), const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** return keys as an ordered set - ordering is by key value */ - GTSAM_EXPORT FastSet keys() const; + FastSet keys() const; /** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */ - GTSAM_EXPORT double error(const Values& c) const; + double error(const Values& c) const; /** Unnormalized probability. O(n) */ - GTSAM_EXPORT double probPrime(const Values& c) const; + double probPrime(const Values& c) const; /** * Create a symbolic factor graph using an existing ordering */ - //GTSAM_EXPORT SymbolicFactorGraph::shared_ptr symbolic() const; + //SymbolicFactorGraph::shared_ptr symbolic() const; /** * Create a symbolic factor graph and initial variable ordering that can @@ -113,13 +113,13 @@ namespace gtsam { * The graph and ordering should be permuted after such a fill-reducing * ordering is found. */ - //GTSAM_EXPORT std::pair + //std::pair // symbolic(const Values& config) const; /** * Compute a fill-reducing ordering using COLAMD. */ - GTSAM_EXPORT Ordering orderingCOLAMD() const; + Ordering orderingCOLAMD() const; /** * Compute a fill-reducing ordering with constraints using CCOLAMD @@ -129,17 +129,17 @@ namespace gtsam { * indices need to appear in the constraints, unconstrained is assumed for all * other variables */ - GTSAM_EXPORT Ordering orderingCOLAMDConstrained(const FastMap& constraints) const; + Ordering orderingCOLAMDConstrained(const FastMap& constraints) const; /** * linearize a nonlinear factor graph */ - GTSAM_EXPORT boost::shared_ptr linearize(const Values& linearizationPoint) const; + boost::shared_ptr linearize(const Values& linearizationPoint) const; /** * Clone() performs a deep-copy of the graph, including all of the factors */ - GTSAM_EXPORT NonlinearFactorGraph clone() const; + NonlinearFactorGraph clone() const; /** * Rekey() performs a deep-copy of all of the factors, and changes @@ -150,7 +150,7 @@ namespace gtsam { * @param rekey_mapping is a map of old->new keys * @result a cloned graph with updated keys */ - GTSAM_EXPORT NonlinearFactorGraph rekey(const std::map& rekey_mapping) const; + NonlinearFactorGraph rekey(const std::map& rekey_mapping) const; private: diff --git a/gtsam/nonlinear/NonlinearISAM.cpp b/gtsam/nonlinear/NonlinearISAM.cpp index 413d554eb..5e8f1f1af 100644 --- a/gtsam/nonlinear/NonlinearISAM.cpp +++ b/gtsam/nonlinear/NonlinearISAM.cpp @@ -15,6 +15,8 @@ * @author Viorela Ila and Richard Roberts */ +#if 0 + #include #include @@ -122,3 +124,5 @@ void NonlinearISAM::printStats() const { /* ************************************************************************* */ }///\ namespace gtsam + +#endif diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index 02afcd3d4..9bf7f8bff 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -15,6 +15,8 @@ * @author Viorela Ila and Richard Roberts */ +#if 0 + #pragma once #include @@ -116,3 +118,4 @@ public: } // \namespace gtsam +#endif diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp index b4f099b0a..67c1c08d1 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp @@ -63,14 +63,16 @@ VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const Succ delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize(); } else if ( params.isCG() ) { - if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ..."); - if ( boost::dynamic_pointer_cast(params.iterativeParams) ) { - SubgraphSolver solver (gfg, *boost::dynamic_pointer_cast(params.iterativeParams)); - delta = solver.optimize(); - } - else { - throw std::runtime_error("solveGaussianFactorGraph: special cg parameter type is not handled in LM solver ..."); - } + throw std::runtime_error("Not implemented"); + + //if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ..."); + //if ( boost::dynamic_pointer_cast(params.iterativeParams) ) { + // SubgraphSolver solver (gfg, *boost::dynamic_pointer_cast(params.iterativeParams)); + // delta = solver.optimize(); + //} + //else { + // throw std::runtime_error("solveGaussianFactorGraph: special cg parameter type is not handled in LM solver ..."); + //} } else { throw std::runtime_error("solveGaussianFactorGraph: Optimization parameter is invalid"); diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h index cb4bdc1b6..589cbd908 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h @@ -20,10 +20,15 @@ #include #include +#include +#include +#include #include namespace gtsam { + // Forward declarations + class GTSAM_EXPORT SuccessiveLinearizationParams : public NonlinearOptimizerParams { public: /** See SuccessiveLinearizationParams::linearSolverType */ @@ -55,19 +60,20 @@ public: virtual void print(const std::string& str) const; - GaussianFactorGraphOrdered::Eliminate getEliminationFunction() const { + GaussianFactorGraph::Eliminate getEliminationFunction() const { switch (linearSolverType) { case MULTIFRONTAL_CHOLESKY: case SEQUENTIAL_CHOLESKY: - return EliminatePreferCholeskyOrdered; + throw std::runtime_error("Not implemented"); + //return EliminatePreferCholeskyOrdered; case MULTIFRONTAL_QR: case SEQUENTIAL_QR: - return EliminateQROrdered; + return EliminateQR; default: throw std::runtime_error("Nonlinear optimization parameter \"factorization\" is invalid"); - return EliminateQROrdered; + return EliminateQR; break; } } diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index f694a9fad..057249ed7 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -82,7 +82,7 @@ namespace gtsam { Values result; for(const_iterator key_value = begin(); key_value != end(); ++key_value) { - const Vector& singleDelta = delta[ordering[key_value->key]]; // Delta for this value + const Vector& singleDelta = delta[key_value->key]; // Delta for this value Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract result.values_.insert(key, retractedValue); // Add retracted result directly to result values diff --git a/gtsam/nonlinear/summarization.cpp b/gtsam/nonlinear/summarization.cpp index b47e559ca..9a2dad1b1 100644 --- a/gtsam/nonlinear/summarization.cpp +++ b/gtsam/nonlinear/summarization.cpp @@ -5,6 +5,8 @@ * @author Alex Cunningham */ +#if 0 + #include #include #include @@ -84,3 +86,4 @@ NonlinearFactorGraph summarizeAsNonlinearContainer( /* ************************************************************************* */ } // \namespace gtsam +#endif diff --git a/gtsam/nonlinear/summarization.h b/gtsam/nonlinear/summarization.h index 0b55cc820..68bad9b34 100644 --- a/gtsam/nonlinear/summarization.h +++ b/gtsam/nonlinear/summarization.h @@ -9,6 +9,8 @@ #pragma once +#if 0 + #include #include @@ -58,4 +60,4 @@ NonlinearFactorGraph GTSAM_EXPORT summarizeAsNonlinearContainer( } // \namespace gtsam - +#endif diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index e22d0b42c..36d26f308 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -179,7 +179,7 @@ pair load2D( noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std)); - graph->add(BearingRangeFactor(id1, id2, bearing, range, measurementNoise)); + *graph += BearingRangeFactor(id1, id2, bearing, range, measurementNoise); // Insert poses or points if they do not exist yet if (!initial->exists(id1)) @@ -222,7 +222,7 @@ pair load2D( } // Add to graph - graph->add(BearingRangeFactor(id1, L(id2), bearing, range, measurementNoise)); + *graph += BearingRangeFactor(id1, L(id2), bearing, range, measurementNoise); } is.ignore(LINESIZE, '\n'); } @@ -386,7 +386,7 @@ pair load2D_robust( noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std)); - graph->add(BearingRangeFactor(id1, id2, bearing, range, measurementNoise)); + *graph += BearingRangeFactor(id1, id2, bearing, range, measurementNoise); // Insert poses or points if they do not exist yet if (!initial->exists(id1))