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

68
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,39 +23,40 @@
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 SymmetricBlockMatrix::LikeActiveViewOf(
{ const VerticalBlockMatrix& 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;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial(DenseIndex nFrontals) VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial(
{ DenseIndex nFrontals) {
// Do dense elimination // Do dense elimination
if(!blockStart() == 0) if (!blockStart() == 0)
throw std::invalid_argument("Can only do Cholesky when the SymmetricBlockMatrix is not a restricted view, i.e. when blockStart == 0."); throw std::invalid_argument(
if(!gtsam::choleskyPartial(matrix_, offset(nFrontals))) "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(); throw CholeskyFailed();
// Split conditional // Split conditional
@ -74,5 +75,8 @@ namespace gtsam {
gttoc(Remaining_factor); gttoc(Remaining_factor);
return Ab; return Ab;
}
} }
/* ************************************************************************* */
} //\ namespace gtsam