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

Small matrix optimization and improved some timing statements

release/4.3a0
Richard Roberts 2013-08-16 00:46:51 +00:00
parent 3701dc6322
commit 66eb9c9964
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/linear/HessianFactor.cpp
View File

@ -64,6 +64,7 @@ string SlotEntry::toString() const {
/* ************************************************************************* */ /* ************************************************************************* */
Scatter::Scatter(const GaussianFactorGraph& gfg, boost::optional<const Ordering&> ordering) Scatter::Scatter(const GaussianFactorGraph& gfg, boost::optional<const Ordering&> ordering)
{ {
gttic(Scatter_Constructor);
static const size_t none = std::numeric_limits<size_t>::max(); static const size_t none = std::numeric_limits<size_t>::max();
// First do the set union. // First do the set union.
@ -223,13 +224,15 @@ GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), ListOfOne(
namespace { namespace {
void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info)
{ {
gttic(HessianFactor_fromJacobian);
const SharedDiagonal& jfModel = jf.get_model(); const SharedDiagonal& jfModel = jf.get_model();
if(jfModel) if(jfModel)
{ {
if(jf.get_model()->isConstrained()) if(jf.get_model()->isConstrained())
throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model");
info.full().noalias() = jf.matrixObject().full().transpose() * jfModel->invsigmas().asDiagonal() * info.full().noalias() = jf.matrixObject().full().transpose() *
jfModel->invsigmas().asDiagonal() * jf.matrixObject().full(); (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() *
jf.matrixObject().full();
} else { } else {
info.full().noalias() = jf.matrixObject().full().transpose() * jf.matrixObject().full(); info.full().noalias() = jf.matrixObject().full().transpose() * jf.matrixObject().full();
} }
@ -373,6 +376,7 @@ double HessianFactor::error(const VectorValues& c) const {
/* ************************************************************************* */ /* ************************************************************************* */
void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter)
{ {
gttic(updateATA);
// This function updates 'combined' with the information in 'update'. 'scatter' maps variables in // This function updates 'combined' with the information in 'update'. 'scatter' maps variables in
// the update factor to slots in the combined factor. // the update factor to slots in the combined factor.