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

Format using BORG conventions

release/4.3a0
dellaert 2014-02-15 11:47:30 -05:00
parent 6a8084ee02
commit b7cbc7a66f
1 changed files with 67 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
gtsam/base/SymmetricBlockMatrix.cpp
View File

@ -1,20 +1,20 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file SymmetricBlockMatrix.cpp * @file SymmetricBlockMatrix.cpp
* @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional. * @brief Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional.
* @author Richard Roberts * @author Richard Roberts
* @date Sep 18, 2010 * @date Sep 18, 2010
*/ */
#include <gtsam/base/SymmetricBlockMatrix.h> #include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/VerticalBlockMatrix.h> #include <gtsam/base/VerticalBlockMatrix.h>
@ -23,56 +23,60 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(const SymmetricBlockMatrix& other) SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(
{ const SymmetricBlockMatrix& other) {
SymmetricBlockMatrix result; SymmetricBlockMatrix result;
result.variableColOffsets_.resize(other.nBlocks() + 1); result.variableColOffsets_.resize(other.nBlocks() + 1);
for(size_t i = 0; i < result.variableColOffsets_.size(); ++i) for (size_t i = 0; i < result.variableColOffsets_.size(); ++i)
result.variableColOffsets_[i] = result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_
other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_]; + i] - other.variableColOffsets_[other.blockStart_];
result.matrix_.resize(other.cols(), other.cols()); result.matrix_.resize(other.cols(), other.cols());
result.assertInvariants(); result.assertInvariants();
return result; return result;
}
/* ************************************************************************* */
SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(const VerticalBlockMatrix& other)
{
SymmetricBlockMatrix result;
result.variableColOffsets_.resize(other.nBlocks() + 1);
for(size_t i = 0; i < result.variableColOffsets_.size(); ++i)
result.variableColOffsets_[i] =
other.variableColOffsets_[other.blockStart_ + i] - other.variableColOffsets_[other.blockStart_];
result.matrix_.resize(other.cols(), other.cols());
result.assertInvariants();
return result;
}
/* ************************************************************************* */
VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial(DenseIndex nFrontals)
{
// Do dense elimination
if(!blockStart() == 0)
throw std::invalid_argument("Can only do Cholesky when the SymmetricBlockMatrix is not a restricted view, i.e. when blockStart == 0.");
if(!gtsam::choleskyPartial(matrix_, offset(nFrontals)))
throw CholeskyFailed();
// Split conditional
// Create one big conditionals with many frontal variables.
gttic(Construct_eliminated);
const size_t varDim = offset(nFrontals);
VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim);
Ab.full() = matrix_.topRows(varDim);
Ab.full().triangularView<Eigen::StrictlyLower>().setZero();
gttoc(Construct_conditional);
gttic(Remaining_factor);
// Take lower-right block of Ab_ to get the remaining factor
blockStart() = nFrontals;
gttoc(Remaining_factor);
return Ab;
}
} }
/* ************************************************************************* */
SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(
const VerticalBlockMatrix& other) {
SymmetricBlockMatrix result;
result.variableColOffsets_.resize(other.nBlocks() + 1);
for (size_t i = 0; i < result.variableColOffsets_.size(); ++i)
result.variableColOffsets_[i] = other.variableColOffsets_[other.blockStart_
+ i] - other.variableColOffsets_[other.blockStart_];
result.matrix_.resize(other.cols(), other.cols());
result.assertInvariants();
return result;
}
/* ************************************************************************* */
VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial(
DenseIndex nFrontals) {
// Do dense elimination
if (!blockStart() == 0)
throw std::invalid_argument(
"Can only do Cholesky when the SymmetricBlockMatrix is not a restricted view, i.e. when blockStart == 0.");
if (!gtsam::choleskyPartial(matrix_, offset(nFrontals)))
throw CholeskyFailed();
// Split conditional
// Create one big conditionals with many frontal variables.
gttic(Construct_eliminated);
const size_t varDim = offset(nFrontals);
VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim);
Ab.full() = matrix_.topRows(varDim);
Ab.full().triangularView<Eigen::StrictlyLower>().setZero();
gttoc(Construct_conditional);
gttic(Remaining_factor);
// Take lower-right block of Ab_ to get the remaining factor
blockStart() = nFrontals;
gttoc(Remaining_factor);
return Ab;
}
/* ************************************************************************* */
} //\ namespace gtsam