12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Started on HessianFactor, converted Nonlinear stuff and disabled a lot of more advanced classes so the library compiles with nonlinear using unordered.

release/4.3a0
Richard Roberts 2013-08-01 21:57:43 +00:00
parent c868056c22
commit 0d05bf5ac5
42 changed files with 2103 additions and 174 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
CMakeLists.txt
View File

@ -103,6 +103,7 @@ endif()
if(MSVC AND NOT Boost_USE_STATIC_LIBS) if(MSVC AND NOT Boost_USE_STATIC_LIBS)
add_definitions(-DBOOST_ALL_DYN_LINK) add_definitions(-DBOOST_ALL_DYN_LINK)
endif() endif()
add_definitions(-DBOOST_ALL_NO_LIB)
find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time regex timer chrono) 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) 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) # 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) if (GTSAM_DISABLE_NEW_TIMERS)
message("WARNING: GTSAM timing instrumentation manually disabled") message("WARNING: GTSAM timing instrumentation manually disabled")
add_definitions(-DGTSAM_DISABLE_NEW_TIMERS) add_definitions(-DGTSAM_DISABLE_NEW_TIMERS)

48
gtsam/base/SymmetricBlockMatrix.cpp Normal file
View File

@ -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 <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/VerticalBlockMatrix.h>
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;
}
}

229
gtsam/base/SymmetricBlockMatrix.h Normal file
View File

@ -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 <gtsam/base/Matrix.h>
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<Matrix> Block;
typedef Eigen::Block<const Matrix> constBlock;
protected:
Matrix matrix_; ///< The full matrix
std::vector<DenseIndex> 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<typename CONTAINER>
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<typename ITERATOR>
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<typename CONTAINER>
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<typename ITERATOR>
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<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(matrix_);
ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
ar & BOOST_SERIALIZATION_NVP(blockStart_);
}
};
}

7
gtsam/base/VerticalBlockMatrix.cpp
View File

@ -21,11 +21,12 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& rhs) { VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& rhs)
{
VerticalBlockMatrix result; VerticalBlockMatrix result;
result.variableColOffsets_.resize(rhs.nBlocks() + 1); result.variableColOffsets_.resize(rhs.nBlocks() + 1);
for(Index i = 0; i < rhs.nBlocks() + 1; ++i) std::copy(rhs.variableColOffsets_.begin() + rhs.blockStart_, rhs.variableColOffsets_.end(),
result.variableColOffsets_[i] = rhs.variableColOffsets_[i + rhs.blockStart_]; result.variableColOffsets_.begin());
result.matrix_.resize(rhs.rows(), result.variableColOffsets_.back()); result.matrix_.resize(rhs.rows(), result.variableColOffsets_.back());
result.rowEnd_ = rhs.rows(); result.rowEnd_ = rhs.rows();
result.assertInvariants(); result.assertInvariants();

103
gtsam/base/VerticalBlockMatrix.h
View File

@ -12,7 +12,7 @@
/** /**
* @file VerticalBlockMatrix.h * @file VerticalBlockMatrix.h
* @brief A matrix with column blocks of pre-defined sizes. Used in JacobianFactor and * @brief A matrix with column blocks of pre-defined sizes. Used in JacobianFactor and
* GaussianConditional. * GaussianConditional.
* @author Richard Roberts * @author Richard Roberts
* @date Sep 18, 2010 */ * @date Sep 18, 2010 */
#pragma once #pragma once
@ -22,40 +22,35 @@
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations
//class S class SymmetricBlockMatrix;
/** /**
* This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks. * This class stores a dense matrix and allows it to be accessed as a collection of vertical
* It also provides for accessing individual columns from those blocks. When constructed or * blocks. The dimensions of the blocks are provided when constructing this class.
* resized, the caller must provide the dimensions of the blocks.
* *
* This class also has three parameters that can be changed after construction that change the * 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 * 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() * 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() * determines the apparent first row of the matrix for all operations (except for setting
* and rowEnd()). rowEnd() determines the apparent exclusive (one-past-the-last) last row for all * rowStart() and rowEnd()). rowEnd() determines the apparent exclusive (one-past-the-last) last
* operations. To include all rows, rowEnd() should be set to the number of rows in the matrix * row for all operations. To include all rows, rowEnd() should be set to the number of rows in
* (i.e. one after the last true row index). * the matrix (i.e. one after the last true row index).
* *
* @addtogroup base */ * @addtogroup base */
class GTSAM_EXPORT VerticalBlockMatrix { class GTSAM_EXPORT VerticalBlockMatrix
{
public: public:
typedef VerticalBlockMatrix This; typedef VerticalBlockMatrix This;
typedef Matrix::Index Index;
typedef Eigen::Block<Matrix> Block; typedef Eigen::Block<Matrix> Block;
typedef Eigen::Block<const Matrix> constBlock; typedef Eigen::Block<const Matrix> constBlock;
// columns of blocks
typedef Eigen::Block<Matrix>::ColXpr Column;
typedef Eigen::Block<const Matrix>::ConstColXpr constColumn;
protected: protected:
Matrix matrix_; ///< The full matrix Matrix matrix_; ///< The full matrix
std::vector<Index> variableColOffsets_; ///< the starting columns of each block (0-based) std::vector<DenseIndex> variableColOffsets_; ///< the starting columns of each block (0-based)
Index rowStart_; ///< Changes apparent matrix view, see main class comment. DenseIndex rowStart_; ///< Changes apparent matrix view, see main class comment.
Index rowEnd_; ///< Changes apparent matrix view, see main class comment. DenseIndex rowEnd_; ///< Changes apparent matrix view, see main class comment.
Index blockStart_; ///< Changes apparent matrix view, see main class comment. DenseIndex blockStart_; ///< Changes apparent matrix view, see main class comment.
public: public:
@ -64,10 +59,10 @@ namespace gtsam {
rowStart_(0), rowEnd_(0), blockStart_(0) rowStart_(0), rowEnd_(0), blockStart_(0)
{ {
variableColOffsets_.push_back(0); variableColOffsets_.push_back(0);
assertInvariants();
} }
/** Construct from a container of the sizes of each vertical block, resize the matrix so that /** Construct from a container of the sizes of each vertical block. */
* its height is matrixNewHeight and its width fits the given block dimensions. */
template<typename CONTAINER> template<typename CONTAINER>
VerticalBlockMatrix(const CONTAINER dimensions, DenseIndex height) : VerticalBlockMatrix(const CONTAINER dimensions, DenseIndex height) :
rowStart_(0), rowEnd_(height), blockStart_(0) 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 * Construct from iterator over the sizes of each vertical block. */
* height is matrixNewHeight and its width fits the given block dimensions. */
template<typename ITERATOR> template<typename ITERATOR>
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height) : VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height) :
rowStart_(0), rowEnd_(height), blockStart_(0) rowStart_(0), rowEnd_(height), blockStart_(0)
@ -101,44 +95,44 @@ namespace gtsam {
} }
/// Row size /// Row size
Index rows() const { assertInvariants(); return rowEnd_ - rowStart_; } DenseIndex rows() const { assertInvariants(); return rowEnd_ - rowStart_; }
/// Column size /// Column size
Index cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; } DenseIndex cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
/// Block count /// 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 */ /** 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 */ /** 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 */ /** access ranges of blocks at a time */
Block range(Index startBlock, Index endBlock) { Block range(DenseIndex startBlock, DenseIndex endBlock) {
assertInvariants(); assertInvariants();
Index actualStartBlock = startBlock + blockStart_; DenseIndex actualStartBlock = startBlock + blockStart_;
Index actualEndBlock = endBlock + blockStart_; DenseIndex actualEndBlock = endBlock + blockStart_;
if(startBlock != 0 || endBlock != 0) { if(startBlock != 0 || endBlock != 0) {
checkBlock(actualStartBlock); checkBlock(actualStartBlock);
assert(actualEndBlock < (DenseIndex)variableColOffsets_.size()); assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
} }
const Index startCol = variableColOffsets_[actualStartBlock]; const DenseIndex startCol = variableColOffsets_[actualStartBlock];
const Index rangeCols = variableColOffsets_[actualEndBlock] - startCol; const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
return matrix_.block(rowStart_, startCol, this->rows(), rangeCols); 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(); assertInvariants();
Index actualStartBlock = startBlock + blockStart_; DenseIndex actualStartBlock = startBlock + blockStart_;
Index actualEndBlock = endBlock + blockStart_; DenseIndex actualEndBlock = endBlock + blockStart_;
if(startBlock != 0 || endBlock != 0) { if(startBlock != 0 || endBlock != 0) {
checkBlock(actualStartBlock); checkBlock(actualStartBlock);
assert(actualEndBlock < (DenseIndex)variableColOffsets_.size()); assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
} }
const Index startCol = variableColOffsets_[actualStartBlock]; const DenseIndex startCol = variableColOffsets_[actualStartBlock];
const Index rangeCols = variableColOffsets_[actualEndBlock] - startCol; const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
return ((const Matrix&)matrix_).block(rowStart_, startCol, this->rows(), rangeCols); 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() */ /** Return the full matrix, *not* including any portions excluded by rowStart(), rowEnd(), and firstBlock() */
const constBlock full() const { return range(0, nBlocks()); } const constBlock full() const { return range(0, nBlocks()); }
Index offset(Index block) const { DenseIndex offset(DenseIndex block) const {
assertInvariants(); assertInvariants();
Index actualBlock = block + blockStart_; DenseIndex actualBlock = block + blockStart_;
checkBlock(actualBlock); checkBlock(actualBlock);
return variableColOffsets_[actualBlock]; return variableColOffsets_[actualBlock];
} }
/** Get or set the apparent first row of the underlying matrix for all operations */ /** 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 */ /** 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 */ /** 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 */ /** 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 */ /** 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 */ /** 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()) */ /** Access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */
const Matrix& matrix() const { return matrix_; } 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()) */ /** Non-const access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */
Matrix& matrix() { return matrix_; } Matrix& matrix() { return matrix_; }
/** /** Copy the block structure and resize the underlying matrix, but do not copy the matrix data.
* Copy the block structure and resize the underlying matrix, but do not * If blockStart(), rowStart(), and/or rowEnd() have been modified, this copies the structure of
* copy the matrix data. If blockStart(), rowStart(), and/or rowEnd() have * the corresponding matrix view. In the destination VerticalBlockView, blockStart() and
* been modified, this copies the structure of the corresponding matrix view. * rowStart() will thus be 0, rowEnd() will be cols() of the source VerticalBlockView, and the
* In the destination VerticalBlockView, blockStart() and rowStart() will * underlying matrix will be the size of the view of the source matrix. */
* 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); static VerticalBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& rhs);
protected: protected:
@ -208,14 +199,14 @@ namespace gtsam {
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) { void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) {
variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1); variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1);
variableColOffsets_[0] = 0; variableColOffsets_[0] = 0;
Index j=0; DenseIndex j=0;
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
++ j; ++ j;
} }
} }
//friend class SymmetricBlockView; friend class SymmetricBlockMatrix;
private: private:
/** Serialization function */ /** Serialization function */

6
gtsam/linear/GaussianFactor.h
View File

@ -67,7 +67,7 @@ namespace gtsam {
* \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also
* GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian. * 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$, * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$,
@ -76,7 +76,7 @@ namespace gtsam {
* GaussianFactorGraph::augmentedJacobian and * GaussianFactorGraph::augmentedJacobian and
* GaussianFactorGraph::sparseJacobian. * GaussianFactorGraph::sparseJacobian.
*/ */
virtual std::pair<Matrix,Vector> jacobian(bool weight = true) const = 0; virtual std::pair<Matrix,Vector> jacobian() const = 0;
/** Return the augmented information matrix represented by this GaussianFactor. /** Return the augmented information matrix represented by this GaussianFactor.
* The augmented information matrix contains the information matrix with an * The augmented information matrix contains the information matrix with an
@ -104,7 +104,7 @@ namespace gtsam {
* stored stored in this factor. * stored stored in this factor.
* @return a HessianFactor with negated Hessian matrices * @return a HessianFactor with negated Hessian matrices
*/ */
//virtual GaussianFactor::shared_ptr negate() const = 0; virtual GaussianFactor::shared_ptr negate() const = 0;
private: private:
/** Serialization function */ /** Serialization function */

539
gtsam/linear/HessianFactor.cpp Normal file
View File

@ -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 <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/tuple/tuple.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/join.hpp>
#include <sstream>
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<Key>& js, const std::vector<Matrix>& Gs,
const std::vector<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<const JacobianFactorOrdered*>(&gf)) {
// const JacobianFactorOrdered& jf(static_cast<const JacobianFactorOrdered&>(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<const HessianFactorOrdered*>(&gf)) {
// const HessianFactorOrdered& hf(static_cast<const HessianFactorOrdered&>(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<size_t> 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<HessianFactorOrdered>(factor))
// updateATA(*hessian, scatter);
// else if(JacobianFactorOrdered::shared_ptr jacobianFactor = boost::dynamic_pointer_cast<JacobianFactorOrdered>(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<Eigen::Upper>()), "Ab^T * Ab: ");
}
/* ************************************************************************* */
bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
if(!dynamic_cast<const HessianFactor*>(&lf))
return false;
else {
Matrix thisMatrix = this->info_.full().selfadjointView<Eigen::Upper>();
thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
Matrix rhsMatrix = static_cast<const HessianFactor&>(lf).info_.full().selfadjointView<Eigen::Upper>();
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<Eigen::Upper>();
}
/* ************************************************************************* */
Matrix HessianFactor::information() const {
return info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>();
}
/* ************************************************************************* */
Matrix HessianFactor::augmentedJacobian() const
{
throw std::runtime_error("Not implemented");
}
/* ************************************************************************* */
std::pair<Matrix, Vector> 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<Eigen::Upper>() * 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; j2<update.info_.nBlocks(); ++j2) {
// size_t slot2 = (j2 == update.size()) ? this->info_.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<Eigen::Upper>() +=
// 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<const JacobianFactorOrdered::AbMatrix> 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<noiseModel::Unit>(update.model_)) {
// updateInform.noalias() = updateA.transpose() * updateA;
//} else {
// noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(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; j2<update.Ab_.nBlocks(); ++j2) {
// size_t slot2 = (j2 == update.size()) ? this->info_.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<Eigen::Upper>() +=
// 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<Matrix> 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<ConditionalType>(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<Index> 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<Index> js;
//std::vector<Matrix> Gs;
//std::vector<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

402
gtsam/linear/HessianFactor.h Normal file
View File

@ -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 <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/linear/GaussianFactor.h>
#include <vector>
// 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<Index, SlotEntry> {
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<This> 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<Key>& js, const std::vector<Matrix>& Gs,
const std::vector<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<HessianFactor>(*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 <em>upper-triangular part</em> 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 <em>upper-triangular part</em> 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 <em>upper-triangular part</em> 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 <em>upper-triangular part</em> 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<Matrix, Vector> 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<GaussianConditional>, boost::shared_ptr<GaussianFactor> >
EliminateCholeskyOrdered(const GaussianFactorGraph& factors, const Ordering& keys);
private:
/** split partially eliminated factor */
boost::shared_ptr<GaussianConditional> splitEliminatedFactor(size_t nrFrontals);
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor);
ar & BOOST_SERIALIZATION_NVP(info_);
}
};
}

18
gtsam/linear/HessianFactorOrdered.cpp
View File

@ -47,19 +47,19 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
string SlotEntry::toString() const { string SlotEntryOrdered::toString() const {
ostringstream oss; ostringstream oss;
oss << "SlotEntry: slot=" << slot << ", dim=" << dimension; oss << "SlotEntryOrdered: slot=" << slot << ", dim=" << dimension;
return oss.str(); return oss.str();
} }
/* ************************************************************************* */ /* ************************************************************************* */
Scatter::Scatter(const FactorGraphOrdered<GaussianFactorOrdered>& gfg) { ScatterOrdered::ScatterOrdered(const FactorGraphOrdered<GaussianFactorOrdered>& gfg) {
// First do the set union. // First do the set union.
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, gfg) { BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, gfg) {
if(factor) { if(factor) {
for(GaussianFactorOrdered::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) { 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<GaussianFactorOrdered>& factors) : info_(matrix_) HessianFactorOrdered::HessianFactorOrdered(const FactorGraphOrdered<GaussianFactorOrdered>& factors) : info_(matrix_)
{ {
Scatter scatter(factors); ScatterOrdered scatter(factors);
// Pull out keys and dimensions // Pull out keys and dimensions
gttic(keys); gttic(keys);
vector<size_t> dimensions(scatter.size() + 1); vector<size_t> 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; dimensions[var_slot.second.slot] = var_slot.second.dimension;
} }
// This is for the r.h.s. vector // This is for the r.h.s. vector
@ -288,7 +288,7 @@ HessianFactorOrdered::HessianFactorOrdered(const FactorGraphOrdered<GaussianFact
info_.resize(dimensions.begin(), dimensions.end(), false); info_.resize(dimensions.begin(), dimensions.end(), false);
// Fill in keys // Fill in keys
keys_.resize(scatter.size()); keys_.resize(scatter.size());
std::transform(scatter.begin(), scatter.end(), keys_.begin(), boost::bind(&Scatter::value_type::first, ::_1)); std::transform(scatter.begin(), scatter.end(), keys_.begin(), boost::bind(&ScatterOrdered::value_type::first, ::_1));
gttoc(allocate); gttoc(allocate);
gttic(zero); gttic(zero);
matrix_.noalias() = Matrix::Zero(matrix_.rows(),matrix_.cols()); matrix_.noalias() = Matrix::Zero(matrix_.rows(),matrix_.cols());
@ -367,7 +367,7 @@ double HessianFactorOrdered::error(const VectorValuesOrdered& c) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void HessianFactorOrdered::updateATA(const HessianFactorOrdered& update, const Scatter& scatter) { void HessianFactorOrdered::updateATA(const HessianFactorOrdered& update, const ScatterOrdered& scatter) {
// This function updates 'combined' with the information in 'update'. // This function updates 'combined' with the information in 'update'.
// 'scatter' maps variables in the update factor to slots in the combined // 'scatter' maps variables in the update factor to slots in the combined
@ -420,7 +420,7 @@ void HessianFactorOrdered::updateATA(const HessianFactorOrdered& update, const S
} }
/* ************************************************************************* */ /* ************************************************************************* */
void HessianFactorOrdered::updateATA(const JacobianFactorOrdered& update, const Scatter& scatter) { void HessianFactorOrdered::updateATA(const JacobianFactorOrdered& update, const ScatterOrdered& scatter) {
// This function updates 'combined' with the information in 'update'. // This function updates 'combined' with the information in 'update'.
// 'scatter' maps variables in the update factor to slots in the combined // 'scatter' maps variables in the update factor to slots in the combined

16
gtsam/linear/HessianFactorOrdered.h
View File

@ -36,23 +36,23 @@ namespace gtsam {
class GaussianConditionalOrdered; class GaussianConditionalOrdered;
template<class C> class BayesNetOrdered; template<class C> 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 // building a HessianFactor incrementally, to get the keys in the right
// order. // order.
// The "scatter" is a map from global variable indices to slot indices in the // 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 // union of involved variables. We also include the dimensionality of the
// variable. // variable.
struct GTSAM_EXPORT SlotEntry { struct GTSAM_EXPORT SlotEntryOrdered {
size_t slot; size_t slot;
size_t dimension; size_t dimension;
SlotEntry(size_t _slot, size_t _dimension) SlotEntryOrdered(size_t _slot, size_t _dimension)
: slot(_slot), dimension(_dimension) {} : slot(_slot), dimension(_dimension) {}
std::string toString() const; std::string toString() const;
}; };
class Scatter : public FastMap<Index, SlotEntry> { class ScatterOrdered : public FastMap<Index, SlotEntryOrdered> {
public: public:
Scatter() {} ScatterOrdered() {}
Scatter(const FactorGraphOrdered<GaussianFactorOrdered>& gfg); ScatterOrdered(const FactorGraphOrdered<GaussianFactorOrdered>& gfg);
}; };
/** /**
@ -350,14 +350,14 @@ namespace gtsam {
* @param update The JacobianFactor containing the new information to add * @param update The JacobianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor * @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 /** Update the factor by adding the information from the HessianFactor
* (used internally during elimination). * (used internally during elimination).
* @param update The HessianFactor containing the new information to add * @param update The HessianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor * @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 */ /** assert invariants */
void assertInvariants() const; void assertInvariants() const;

6
gtsam/linear/JacobianFactor-inl.h
View File

@ -65,7 +65,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
DenseIndex _getCols(const std::pair<Key,Matrix>& p) { DenseIndex _getColsJF(const std::pair<Key,Matrix>& p) {
return p.second.cols(); return p.second.cols();
} }
} }
@ -86,9 +86,9 @@ namespace gtsam {
// a single '1' to add a dimension for the b vector. // a single '1' to add a dimension for the b vector.
{ {
using boost::adaptors::transformed; using boost::adaptors::transformed;
using boost::join;
using boost::assign::cref_list_of; 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 // Check and add terms

47
gtsam/linear/JacobianFactor.cpp
View File

@ -20,7 +20,7 @@
#include <gtsam/linear/linearExceptions.h> #include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianConditional.h> #include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
//#include <gtsam/linear/HessianFactor.h> #include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/VariableSlots.h> #include <gtsam/inference/VariableSlots.h>
@ -60,15 +60,17 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor() : JacobianFactor::JacobianFactor() :
Ab_(cref_list_of<1>(1), 0) Ab_(cref_list_of<1>(1), 0)
{} {
getb().setZero();
}
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor(const GaussianFactor& gf) { JacobianFactor::JacobianFactor(const GaussianFactor& gf) {
// Copy the matrix data depending on what type of factor we're copying from // Copy the matrix data depending on what type of factor we're copying from
if(const JacobianFactor* rhs = dynamic_cast<const JacobianFactor*>(&gf)) if(const JacobianFactor* rhs = dynamic_cast<const JacobianFactor*>(&gf))
*this = JacobianFactor(*rhs); *this = JacobianFactor(*rhs);
//else if(const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&gf)) else if(const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&gf))
// *this = JacobianFactor(*rhs); *this = JacobianFactor(*rhs);
else else
throw std::invalid_argument("In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor"); throw std::invalid_argument("In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
} }
@ -441,24 +443,33 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
pair<Matrix,Vector> JacobianFactor::jacobian(bool weight) const { pair<Matrix,Vector> JacobianFactor::jacobian() const {
pair<Matrix,Vector> result = jacobianUnweighted();
// divide in sigma so error is indeed 0.5*|Ax-b|
if (model_)
model_->WhitenSystem(result.first, result.second);
return result;
}
/* ************************************************************************* */
pair<Matrix,Vector> JacobianFactor::jacobianUnweighted() const {
Matrix A(Ab_.range(0, size())); Matrix A(Ab_.range(0, size()));
Vector b(getb()); 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); return make_pair(A, b);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix JacobianFactor::augmentedJacobian(bool weight) const { Matrix JacobianFactor::augmentedJacobian() const {
if (weight && model_) { Matrix Ab = augmentedJacobianUnweighted();
Matrix Ab(Ab_.range(0,Ab_.nBlocks())); if (model_) {
model_->WhitenInPlace(Ab); 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 { GaussianFactor::shared_ptr JacobianFactor::negate() const {
// HessianFactor hessian(*this); HessianFactor hessian(*this);
// return hessian.negate(); return hessian.negate();
//} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> > std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> >

31
gtsam/linear/JacobianFactor.h
View File

@ -88,8 +88,8 @@ namespace gtsam {
public: public:
typedef VerticalBlockMatrix::Block ABlock; typedef VerticalBlockMatrix::Block ABlock;
typedef VerticalBlockMatrix::constBlock constABlock; typedef VerticalBlockMatrix::constBlock constABlock;
typedef VerticalBlockMatrix::Column BVector; typedef ABlock::ColXpr BVector;
typedef VerticalBlockMatrix::constColumn constBVector; typedef constABlock::ConstColXpr constBVector;
/** Convert from other GaussianFactor */ /** Convert from other GaussianFactor */
explicit JacobianFactor(const GaussianFactor& gf); explicit JacobianFactor(const GaussianFactor& gf);
@ -178,26 +178,37 @@ namespace gtsam {
* @param ordering of variables needed for matrix column order * @param ordering of variables needed for matrix column order
* @param set weight to true to bake in the weights * @param set weight to true to bake in the weights
*/ */
virtual std::pair<Matrix, Vector> jacobian(bool weight = true) const; virtual std::pair<Matrix, Vector> 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<Matrix, Vector> jacobianUnweighted() const;
/** /**
* Return (dense) matrix associated with factor * Return (dense) matrix associated with factor
* The returned system is an augmented matrix: [A b] * The returned system is an augmented matrix: [A b]
* @param set weight to use whitening to bake in weights * @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 * Construct the corresponding anti-factor to negate information
* stored stored in this factor. * stored stored in this factor.
* @return a HessianFactor with negated Hessian matrices * @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 /** Check if the factor is empty. TODO: How should this be defined? */
* not necessarily mean that the factor involves no variables (to check for
* involving no variables use keys().empty()).
*/
virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; } virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; }
/** is noise model constrained ? */ /** is noise model constrained ? */
@ -284,7 +295,7 @@ namespace gtsam {
* factor to be the remaining component. Performs same operation as eliminate(), * factor to be the remaining component. Performs same operation as eliminate(),
* but without running QR. * but without running QR.
*/ */
boost::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals = 1); boost::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals);
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;

4
gtsam/linear/VectorValues.cpp
View File

@ -250,8 +250,8 @@ namespace gtsam {
VectorValues operator*(const double a, const VectorValues &v) VectorValues operator*(const double a, const VectorValues &v)
{ {
VectorValues result; VectorValues result;
BOOST_FOREACH(const VectorValues::KeyValuePair& v, v) BOOST_FOREACH(const VectorValues::KeyValuePair& key_v, v)
result.values_.insert(result.values_.end(), make_pair(v.first, a * v.second)); result.values_.insert(result.values_.end(), make_pair(key_v.first, a * key_v.second));
return result; return result;
} }

6
gtsam/linear/VectorValues.h
View File

@ -197,11 +197,11 @@ namespace gtsam {
iterator begin() { return values_.begin(); } ///< Iterator over variables iterator begin() { return values_.begin(); } ///< Iterator over variables
const_iterator begin() const { 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 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 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 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. */ /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */

0
gtsam/linear/tests/testHessianFactor.cpp → gtsam/linear/tests/testHessianFactorObsolete.cpp
View File

638
gtsam/linear/tests/testHessianFactorUnordered.cpp Normal file
View File

@ -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 <vector>
#include <utility>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std/map.hpp>
#include <gtsam/base/debug.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
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<pair<Index, Matrix> > 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<size_t>(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<size_t> 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<Eigen::Upper>() * 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<size_t> 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<size_t> 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<size_t> 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<Index> js;
js.push_back(0); js.push_back(1); js.push_back(2);
std::vector<Matrix> 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<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<size_t> 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<HessianFactorOrdered> hessians;
BOOST_FOREACH(const GaussianFactorGraphOrdered::sharedFactor& factor, gfg) {
if(boost::shared_ptr<HessianFactorOrdered> hf = boost::dynamic_pointer_cast<HessianFactorOrdered>(factor))
hessians.push_back(hf);
else if(boost::shared_ptr<JacobianFactorOrdered> jf = boost::dynamic_pointer_cast<JacobianFactorOrdered>(factor))
hessians.push_back(boost::make_shared<HessianFactorOrdered>(*jf));
else
CHECK(false);
}
// Eliminate
GaussianFactorGraphOrdered::EliminationResult actualCholesky = EliminateCholeskyOrdered(gfg, 1);
HessianFactorOrdered::shared_ptr actualFactor = boost::dynamic_pointer_cast<HessianFactorOrdered>(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<pair<Index, Matrix> > 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<HessianFactorOrdered> 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<GaussianFactorOrdered> 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<Eigen::Upper>()),
Matrix(actual.matrix_.triangularView<Eigen::Upper>()), tol));
}
#endif
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

4
gtsam/nonlinear/DoglegOptimizer.cpp
View File

@ -16,6 +16,8 @@
* @date Feb 26, 2012 * @date Feb 26, 2012
*/ */
#if 0
#include <gtsam/nonlinear/DoglegOptimizer.h> #include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h> #include <gtsam/nonlinear/DoglegOptimizerImpl.h>
#include <gtsam/linear/GaussianBayesTree.h> #include <gtsam/linear/GaussianBayesTree.h>
@ -95,3 +97,5 @@ DoglegParams DoglegOptimizer::ensureHasOrdering(DoglegParams params, const Nonli
} }
} /* namespace gtsam */ } /* namespace gtsam */
#endif

4
gtsam/nonlinear/DoglegOptimizer.h
View File

@ -18,6 +18,8 @@
#pragma once #pragma once
#if 0
#include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h> #include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h>
namespace gtsam { namespace gtsam {
@ -162,3 +164,5 @@ protected:
}; };
} }
#endif

2
gtsam/nonlinear/GaussNewtonOptimizer.h
View File

@ -61,7 +61,7 @@ public:
*/ */
GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
const GaussNewtonParams& params = GaussNewtonParams()) : 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 /** Standard constructor, requires a nonlinear factor graph, initial
* variable assignments, and optimization parameters. For convenience this * variable assignments, and optimization parameters. For convenience this

4
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -16,6 +16,8 @@
* @author Richard Roberts * @author Richard Roberts
*/ */
#if 0
#include <gtsam/nonlinear/ISAM2-impl.h> #include <gtsam/nonlinear/ISAM2-impl.h>
#include <gtsam/nonlinear/Symbol.h> // for selective linearization thresholds #include <gtsam/nonlinear/Symbol.h> // for selective linearization thresholds
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
@ -493,3 +495,5 @@ size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThresho
} }
} }
#endif

4
gtsam/nonlinear/ISAM2-impl.h
View File

@ -17,6 +17,8 @@
#pragma once #pragma once
#if 0
#include <gtsam/linear/GaussianBayesTreeOrdered.h> #include <gtsam/linear/GaussianBayesTreeOrdered.h>
#include <gtsam/nonlinear/ISAM2.h> #include <gtsam/nonlinear/ISAM2.h>
@ -154,3 +156,5 @@ struct GTSAM_EXPORT ISAM2::Impl {
}; };
} }
#endif

4
gtsam/nonlinear/ISAM2.cpp
View File

@ -15,6 +15,8 @@
* @author Michael Kaess, Richard Roberts * @author Michael Kaess, Richard Roberts
*/ */
#if 0
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign; using namespace boost::assign;
@ -1150,3 +1152,5 @@ void gradientAtZero(const ISAM2& bayesTree, VectorValuesOrdered& g) {
} }
/// namespace gtsam /// namespace gtsam
#endif

4
gtsam/nonlinear/ISAM2.h
View File

@ -19,6 +19,8 @@
#pragma once #pragma once
#if 0
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h> #include <gtsam/nonlinear/DoglegOptimizerImpl.h>
#include <gtsam/linear/GaussianBayesTreeOrdered.h> #include <gtsam/linear/GaussianBayesTreeOrdered.h>
@ -729,3 +731,5 @@ GTSAM_EXPORT void gradientAtZero(const ISAM2& bayesTree, VectorValuesOrdered& g)
#include <gtsam/nonlinear/ISAM2-inl.h> #include <gtsam/nonlinear/ISAM2-inl.h>
#include <gtsam/nonlinear/ISAM2-impl.h> #include <gtsam/nonlinear/ISAM2-impl.h>
#endif

5
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -16,6 +16,8 @@
* @date Feb 26, 2012 * @date Feb 26, 2012
*/ */
#if 0
#include <cmath> #include <cmath>
#include <gtsam/linear/linearExceptions.h> #include <gtsam/linear/linearExceptions.h>
@ -179,3 +181,6 @@ LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering(
} }
} /* namespace gtsam */ } /* namespace gtsam */
#endif

8
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

@ -18,6 +18,8 @@
#pragma once #pragma once
#if 0
#include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h> #include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h>
namespace gtsam { namespace gtsam {
@ -112,7 +114,7 @@ public:
*/ */
LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
const LevenbergMarquardtParams& params = LevenbergMarquardtParams()) : 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)) {} state_(graph, initialValues, params_), dimensions_(initialValues.dims(*params_.ordering)) {}
/** Standard constructor, requires a nonlinear factor graph, initial /** Standard constructor, requires a nonlinear factor graph, initial
@ -122,7 +124,7 @@ public:
* @param graph The nonlinear factor graph to optimize * @param graph The nonlinear factor graph to optimize
* @param initialValues The initial variable assignments * @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)) { NonlinearOptimizer(graph), dimensions_(initialValues.dims(ordering)) {
params_.ordering = ordering; params_.ordering = ordering;
state_ = LevenbergMarquardtState(graph, initialValues, params_); } state_ = LevenbergMarquardtState(graph, initialValues, params_); }
@ -176,3 +178,5 @@ protected:
}; };
} }
#endif

18
gtsam/nonlinear/LinearContainerFactor.cpp
View File

@ -7,10 +7,13 @@
#include <gtsam/nonlinear/LinearContainerFactor.h> #include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/linear/HessianFactor.h> #include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#if 0
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
@ -55,18 +58,6 @@ LinearContainerFactor::LinearContainerFactor(
initializeLinearizationPoint(linearizationPoint); 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 { void LinearContainerFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
Base::print(s+"LinearContainerFactor", keyFormatter); 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<HessianFactor>(linFactor); HessianFactor::shared_ptr hesFactor = boost::dynamic_pointer_cast<HessianFactor>(linFactor);
size_t dim = hesFactor->linearTerm().size(); size_t dim = hesFactor->linearTerm().size();
Eigen::Block<HessianFactor::Block> Gview = hesFactor->info().block(0, 0, dim, dim); Eigen::Block<HessianFactor::Block> Gview = hesFactor->info().block(0, 0, dim, dim);
Vector deltaVector = delta.asVector(); Vector deltaVector = delta.vector();
Vector G_delta = Gview.selfadjointView<Eigen::Upper>() * deltaVector; Vector G_delta = Gview.selfadjointView<Eigen::Upper>() * deltaVector;
hesFactor->constantTerm() += deltaVector.dot(G_delta) - 2.0 * deltaVector.dot(hesFactor->linearTerm()); hesFactor->constantTerm() += deltaVector.dot(G_delta) - 2.0 * deltaVector.dot(hesFactor->linearTerm());
hesFactor->linearTerm() -= G_delta; hesFactor->linearTerm() -= G_delta;
@ -192,3 +183,4 @@ NonlinearFactorGraph LinearContainerFactor::convertLinearGraph(
/* ************************************************************************* */ /* ************************************************************************* */
} // \namespace gtsam } // \namespace gtsam
#endif

12
gtsam/nonlinear/LinearContainerFactor.h
View File

@ -11,9 +11,12 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#if 0
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations
class JacobianFactor;
class HessianFactor; class HessianFactor;
/** /**
@ -45,9 +48,6 @@ public:
/** Constructor from shared_ptr */ /** Constructor from shared_ptr */
LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint = Values()); 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 // Access
const GaussianFactor::shared_ptr& factor() const { return factor_; } const GaussianFactor::shared_ptr& factor() const { return factor_; }
@ -129,10 +129,10 @@ public:
bool isHessian() const; bool isHessian() const;
/** Casts to JacobianFactor */ /** Casts to JacobianFactor */
JacobianFactor::shared_ptr toJacobian() const; boost::shared_ptr<JacobianFactor> toJacobian() const;
/** Casts to HessianFactor */ /** Casts to HessianFactor */
HessianFactor::shared_ptr toHessian() const; boost::shared_ptr<HessianFactor> toHessian() const;
/** /**
* Utility function for converting linear graphs to nonlinear graphs * Utility function for converting linear graphs to nonlinear graphs
@ -160,4 +160,4 @@ private:
} // \namespace gtsam } // \namespace gtsam
#endif

4
gtsam/nonlinear/Marginals.cpp
View File

@ -16,6 +16,8 @@
* @date May 14, 2012 * @date May 14, 2012
*/ */
#if 0
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h> #include <gtsam/linear/GaussianMultifrontalSolver.h>
@ -175,3 +177,5 @@ void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) c
} }
} /* namespace gtsam */ } /* namespace gtsam */
#endif

4
gtsam/nonlinear/Marginals.h
View File

@ -18,6 +18,8 @@
#pragma once #pragma once
#if 0
#include <gtsam/base/blockMatrices.h> #include <gtsam/base/blockMatrices.h>
#include <gtsam/linear/GaussianBayesTreeOrdered.h> #include <gtsam/linear/GaussianBayesTreeOrdered.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -141,3 +143,5 @@ protected:
}; };
} /* namespace gtsam */ } /* namespace gtsam */
#endif

4
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp
View File

@ -5,6 +5,8 @@
* @date Jun 11, 2012 * @date Jun 11, 2012
*/ */
#if 0
#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h> #include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
#include <gtsam/nonlinear/OrderingOrdered.h> #include <gtsam/nonlinear/OrderingOrdered.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
@ -62,3 +64,5 @@ const Values& NonlinearConjugateGradientOptimizer::optimize() {
} }
} /* namespace gtsam */ } /* namespace gtsam */
#endif

9
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
View File

@ -7,6 +7,8 @@
#pragma once #pragma once
#if 0
#include <gtsam/base/Manifold.h> #include <gtsam/base/Manifold.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h> #include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
@ -185,9 +187,6 @@ boost::tuple<V, size_t> nonlinearConjugateGradient(const S &system, const V &ini
return boost::tie(currentValues, iteration); return boost::tie(currentValues, iteration);
} }
} }
#endif

2
gtsam/nonlinear/NonlinearFactor.h
View File

@ -305,7 +305,7 @@ public:
this->noiseModel_->WhitenSystem(A,b); this->noiseModel_->WhitenSystem(A,b);
std::vector<std::pair<Index, Matrix> > terms(this->size()); std::vector<std::pair<Key, Matrix> > terms(this->size());
// Fill in terms // Fill in terms
for(size_t j=0; j<this->size(); ++j) { for(size_t j=0; j<this->size(); ++j) {
terms[j].first = this->keys()[j]; terms[j].first = this->keys()[j];

26
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -62,7 +62,7 @@ namespace gtsam {
* tangent vector space at the linearization point. Because the tangent space is a true * 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. * vector space, the config type will be an VectorValues in that linearized factor graph.
*/ */
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor> { class GTSAM_EXPORT NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
public: public:
@ -86,26 +86,26 @@ namespace gtsam {
NonlinearFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {} NonlinearFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
/** print just calls base class */ /** 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 */ /** 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 GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** return keys as an ordered set - ordering is by key value */ /** return keys as an ordered set - ordering is by key value */
GTSAM_EXPORT FastSet<Key> keys() const; FastSet<Key> keys() const;
/** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */ /** 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) */ /** 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 * 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 * 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 * The graph and ordering should be permuted after such a fill-reducing
* ordering is found. * ordering is found.
*/ */
//GTSAM_EXPORT std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> //std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr>
// symbolic(const Values& config) const; // symbolic(const Values& config) const;
/** /**
* Compute a fill-reducing ordering using COLAMD. * Compute a fill-reducing ordering using COLAMD.
*/ */
GTSAM_EXPORT Ordering orderingCOLAMD() const; Ordering orderingCOLAMD() const;
/** /**
* Compute a fill-reducing ordering with constraints using CCOLAMD * 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 * indices need to appear in the constraints, unconstrained is assumed for all
* other variables * other variables
*/ */
GTSAM_EXPORT Ordering orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const; Ordering orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const;
/** /**
* linearize a nonlinear factor graph * linearize a nonlinear factor graph
*/ */
GTSAM_EXPORT boost::shared_ptr<GaussianFactorGraph> linearize(const Values& linearizationPoint) const; boost::shared_ptr<GaussianFactorGraph> linearize(const Values& linearizationPoint) const;
/** /**
* Clone() performs a deep-copy of the graph, including all of the factors * 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 * 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 * @param rekey_mapping is a map of old->new keys
* @result a cloned graph with updated keys * @result a cloned graph with updated keys
*/ */
GTSAM_EXPORT NonlinearFactorGraph rekey(const std::map<Key,Key>& rekey_mapping) const; NonlinearFactorGraph rekey(const std::map<Key,Key>& rekey_mapping) const;
private: private:

4
gtsam/nonlinear/NonlinearISAM.cpp
View File

@ -15,6 +15,8 @@
* @author Viorela Ila and Richard Roberts * @author Viorela Ila and Richard Roberts
*/ */
#if 0
#include <gtsam/nonlinear/NonlinearISAM.h> #include <gtsam/nonlinear/NonlinearISAM.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h> #include <gtsam/linear/GaussianFactorGraphOrdered.h>
@ -122,3 +124,5 @@ void NonlinearISAM::printStats() const {
/* ************************************************************************* */ /* ************************************************************************* */
}///\ namespace gtsam }///\ namespace gtsam
#endif

3
gtsam/nonlinear/NonlinearISAM.h
View File

@ -15,6 +15,8 @@
* @author Viorela Ila and Richard Roberts * @author Viorela Ila and Richard Roberts
*/ */
#if 0
#pragma once #pragma once
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -116,3 +118,4 @@ public:
} // \namespace gtsam } // \namespace gtsam
#endif

18
gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp
View File

@ -63,14 +63,16 @@ VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const Succ
delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize(); delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize();
} }
else if ( params.isCG() ) { else if ( params.isCG() ) {
if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ..."); throw std::runtime_error("Not implemented");
if ( boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams) ) {
SubgraphSolver solver (gfg, *boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams)); //if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ...");
delta = solver.optimize(); //if ( boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams) ) {
} // SubgraphSolver solver (gfg, *boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams));
else { // delta = solver.optimize();
throw std::runtime_error("solveGaussianFactorGraph: special cg parameter type is not handled in LM solver ..."); //}
} //else {
// throw std::runtime_error("solveGaussianFactorGraph: special cg parameter type is not handled in LM solver ...");
//}
} }
else { else {
throw std::runtime_error("solveGaussianFactorGraph: Optimization parameter is invalid"); throw std::runtime_error("solveGaussianFactorGraph: Optimization parameter is invalid");

14
gtsam/nonlinear/SuccessiveLinearizationOptimizer.h
View File

@ -20,10 +20,15 @@
#include <gtsam/nonlinear/NonlinearOptimizer.h> #include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/linear/SubgraphSolver.h> #include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
namespace gtsam { namespace gtsam {
// Forward declarations
class GTSAM_EXPORT SuccessiveLinearizationParams : public NonlinearOptimizerParams { class GTSAM_EXPORT SuccessiveLinearizationParams : public NonlinearOptimizerParams {
public: public:
/** See SuccessiveLinearizationParams::linearSolverType */ /** See SuccessiveLinearizationParams::linearSolverType */
@ -55,19 +60,20 @@ public:
virtual void print(const std::string& str) const; virtual void print(const std::string& str) const;
GaussianFactorGraphOrdered::Eliminate getEliminationFunction() const { GaussianFactorGraph::Eliminate getEliminationFunction() const {
switch (linearSolverType) { switch (linearSolverType) {
case MULTIFRONTAL_CHOLESKY: case MULTIFRONTAL_CHOLESKY:
case SEQUENTIAL_CHOLESKY: case SEQUENTIAL_CHOLESKY:
return EliminatePreferCholeskyOrdered; throw std::runtime_error("Not implemented");
//return EliminatePreferCholeskyOrdered;
case MULTIFRONTAL_QR: case MULTIFRONTAL_QR:
case SEQUENTIAL_QR: case SEQUENTIAL_QR:
return EliminateQROrdered; return EliminateQR;
default: default:
throw std::runtime_error("Nonlinear optimization parameter \"factorization\" is invalid"); throw std::runtime_error("Nonlinear optimization parameter \"factorization\" is invalid");
return EliminateQROrdered; return EliminateQR;
break; break;
} }
} }

2
gtsam/nonlinear/Values.cpp
View File

@ -82,7 +82,7 @@ namespace gtsam {
Values result; Values result;
for(const_iterator key_value = begin(); key_value != end(); ++key_value) { 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 Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument
Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract
result.values_.insert(key, retractedValue); // Add retracted result directly to result values result.values_.insert(key, retractedValue); // Add retracted result directly to result values

3
gtsam/nonlinear/summarization.cpp
View File

@ -5,6 +5,8 @@
* @author Alex Cunningham * @author Alex Cunningham
*/ */
#if 0
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/nonlinear/summarization.h> #include <gtsam/nonlinear/summarization.h>
#include <gtsam/nonlinear/LinearContainerFactor.h> #include <gtsam/nonlinear/LinearContainerFactor.h>
@ -84,3 +86,4 @@ NonlinearFactorGraph summarizeAsNonlinearContainer(
/* ************************************************************************* */ /* ************************************************************************* */
} // \namespace gtsam } // \namespace gtsam
#endif

4
gtsam/nonlinear/summarization.h
View File

@ -9,6 +9,8 @@
#pragma once #pragma once
#if 0
#include <gtsam/dllexport.h> #include <gtsam/dllexport.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -58,4 +60,4 @@ NonlinearFactorGraph GTSAM_EXPORT summarizeAsNonlinearContainer(
} // \namespace gtsam } // \namespace gtsam
#endif

6
gtsam/slam/dataset.cpp
View File

@ -179,7 +179,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::shared_ptr measurementNoise =
noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std)); noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std));
graph->add(BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise)); *graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise);
// Insert poses or points if they do not exist yet // Insert poses or points if they do not exist yet
if (!initial->exists(id1)) if (!initial->exists(id1))
@ -222,7 +222,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
} }
// Add to graph // Add to graph
graph->add(BearingRangeFactor<Pose2, Point2>(id1, L(id2), bearing, range, measurementNoise)); *graph += BearingRangeFactor<Pose2, Point2>(id1, L(id2), bearing, range, measurementNoise);
} }
is.ignore(LINESIZE, '\n'); is.ignore(LINESIZE, '\n');
} }
@ -386,7 +386,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::shared_ptr measurementNoise =
noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std)); noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std));
graph->add(BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise)); *graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise);
// Insert poses or points if they do not exist yet // Insert poses or points if they do not exist yet
if (!initial->exists(id1)) if (!initial->exists(id1))