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

Removed use of boost::range 'join' - replaced with a special flag to add one dimension in VerticalBlockMatrix and SymmetricBlockMatrix

release/4.3a0
Richard Roberts 2014-02-12 18:50:05 -05:00
parent 27a86352d7
commit 7cb4a8390a
5 changed files with 29 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
gtsam/base/SymmetricBlockMatrix.h
View File

@ -61,32 +61,32 @@ namespace gtsam {
/// Construct from a container of the sizes of each block.
template<typename CONTAINER>
SymmetricBlockMatrix(const CONTAINER& dimensions) :
SymmetricBlockMatrix(const CONTAINER& dimensions, bool appendOneDimension = false) :
blockStart_(0)
{
fillOffsets(dimensions.begin(), dimensions.end());
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
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) :
SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension = false) :
blockStart_(0)
{
fillOffsets(firstBlockDim, lastBlockDim);
fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
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) :
SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) :
blockStart_(0)
{
matrix_.resize(matrix.rows(), matrix.cols());
matrix_.triangularView<Eigen::Upper>() = matrix.triangularView<Eigen::Upper>();
fillOffsets(dimensions.begin(), dimensions.end());
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
if(matrix_.rows() != matrix_.cols())
throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from a non-square matrix.");
if(variableColOffsets_.back() != matrix_.cols())
@ -211,85 +211,21 @@ namespace gtsam {
return variableColOffsets_[block + blockStart_];
}
//void checkRange(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const
//{
// const DenseIndex i_actualStartBlock = i_startBlock + blockStart_;
// const DenseIndex i_actualEndBlock = i_endBlock + blockStart_;
// const DenseIndex j_actualStartBlock = j_startBlock + blockStart_;
// const 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());
// }
//}
//void checkRange(DenseIndex startBlock, DenseIndex endBlock) const
//{
// const DenseIndex actualStartBlock = startBlock + blockStart_;
// const DenseIndex actualEndBlock = endBlock + blockStart_;
// checkBlock(actualStartBlock);
// if(startBlock != 0 || endBlock != 0) {
// checkBlock(actualStartBlock);
// assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
// }
//}
//Block rangeUnchecked(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock)
//{
// const DenseIndex i_actualStartBlock = i_startBlock + blockStart_;
// const DenseIndex i_actualEndBlock = i_endBlock + blockStart_;
// const DenseIndex j_actualStartBlock = j_startBlock + blockStart_;
// const DenseIndex j_actualEndBlock = j_endBlock + blockStart_;
// return Block(matrix(),
// variableColOffsets_[i_actualStartBlock],
// variableColOffsets_[j_actualStartBlock],
// variableColOffsets_[i_actualEndBlock] - variableColOffsets_[i_actualStartBlock],
// variableColOffsets_[j_actualEndBlock] - variableColOffsets_[j_actualStartBlock]);
//}
//constBlock rangeUnchecked(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const
//{
// // Convert Block to constBlock
// const Block block = const_cast<This*>(this)->rangeUnchecked(i_startBlock, i_endBlock, j_startBlock, j_endBlock);
// return constBlock(matrix(), block.Base::Base::, block.startCol(), block.rows(), block.cols());
//}
//Block rangeUnchecked(DenseIndex startBlock, DenseIndex endBlock)
//{
// const DenseIndex actualStartBlock = startBlock + blockStart_;
// const DenseIndex actualEndBlock = endBlock + blockStart_;
// return Block(matrix(),
// variableColOffsets_[actualStartBlock],
// variableColOffsets_[actualStartBlock],
// variableColOffsets_[actualEndBlock] - variableColOffsets_[actualStartBlock],
// variableColOffsets_[actualEndBlock] - variableColOffsets_[actualStartBlock]);
//}
//constBlock rangeUnchecked(DenseIndex startBlock, DenseIndex endBlock) const
//{
// // Convert Block to constBlock
// const Block block = const_cast<This*>(this)->rangeUnchecked(startBlock, endBlock);
// return constBlock(matrix(), block.startRow(), block.startCol(), block.rows(), block.cols());
//}
template<typename ITERATOR>
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim)
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension)
{
variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1);
variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
variableColOffsets_[0] = 0;
DenseIndex j=0;
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
++ j;
}
if(appendOneDimension)
{
variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
++ j;
}
}
friend class VerticalBlockMatrix;

21
gtsam/base/VerticalBlockMatrix.h
View File

@ -65,20 +65,20 @@ namespace gtsam {
/** Construct from a container of the sizes of each vertical block. */
template<typename CONTAINER>
VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height) :
VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, bool appendOneDimension = false) :
rowStart_(0), rowEnd_(height), blockStart_(0)
{
fillOffsets(dimensions.begin(), dimensions.end());
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
matrix_.resize(height, variableColOffsets_.back());
assertInvariants();
}
/** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */
template<typename CONTAINER>
VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) :
VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) :
matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0)
{
fillOffsets(dimensions.begin(), dimensions.end());
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
if(variableColOffsets_.back() != matrix_.cols())
throw std::invalid_argument("Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
assertInvariants();
@ -87,10 +87,10 @@ namespace gtsam {
/**
* Construct from iterator over the sizes of each vertical block. */
template<typename ITERATOR>
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height) :
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension = false) :
rowStart_(0), rowEnd_(height), blockStart_(0)
{
fillOffsets(firstBlockDim, lastBlockDim);
fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
matrix_.resize(height, variableColOffsets_.back());
assertInvariants();
}
@ -202,14 +202,19 @@ namespace gtsam {
}
template<typename ITERATOR>
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) {
variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1);
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) {
variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
variableColOffsets_[0] = 0;
DenseIndex j=0;
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
++ j;
}
if(appendOneDimension)
{
variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
++ j;
}
}
friend class SymmetricBlockMatrix;

3
gtsam/linear/HessianFactor.cpp
View File

@ -42,7 +42,6 @@
#include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/join.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <sstream>
@ -179,7 +178,7 @@ DenseIndex _getSizeHF(const Vector& m) { return m.size(); }
/* ************************************************************************* */
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 | br::transformed(&_getSizeHF), ListOfOne((DenseIndex)1)))
GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true)
{
// Get the number of variables
size_t variable_count = js.size();

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

@ -21,7 +21,6 @@
#include <gtsam/linear/linearExceptions.h>
#include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/join.hpp>
#include <boost/range/algorithm/for_each.hpp>
#include <boost/foreach.hpp>
@ -123,7 +122,7 @@ namespace gtsam {
// matrices, then extract the number of columns e.g. dimensions in each matrix. Then joins with
// a single '1' to add a dimension for the b vector.
{
Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&internal::getColsJF), ListOfOne((DenseIndex)1)), b.size());
Ab_ = VerticalBlockMatrix(terms | transformed(&internal::getColsJF), b.size(), true);
}
// Check and add terms

2
gtsam/linear/JacobianFactor.cpp
View File

@ -283,7 +283,7 @@ namespace gtsam {
// Allocate matrix and copy keys in order
gttic(allocate);
Ab_ = VerticalBlockMatrix(boost::join(varDims, ListOfOne((DenseIndex)1)), m); // Allocate augmented matrix
Ab_ = VerticalBlockMatrix(varDims, m, true); // Allocate augmented matrix
Base::keys_.resize(orderedSlots.size());
boost::range::copy( // Get variable keys
orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin());