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

Optimizations converting JacobianFactor to HessianFactor (i.e. forming A^T * A)

release/4.3a0
Richard Roberts 2011-02-07 06:09:16 +00:00
parent cd27192ade
commit fff86f98b5
2 changed files with 19 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/linear/HessianFactor.cpp
View File

@ -150,9 +150,19 @@ namespace gtsam {
// 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(dynamic_cast<const JacobianFactor*>(&gf)) { if(dynamic_cast<const JacobianFactor*>(&gf)) {
const JacobianFactor& jf(static_cast<const JacobianFactor&>(gf)); const JacobianFactor& jf(static_cast<const JacobianFactor&>(gf));
JacobianFactor whitened(jf.whiten()); if(jf.model_->isConstrained())
info_.copyStructureFrom(whitened.Ab_); throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model");
matrix_ = ublas::prod(ublas::trans(whitened.matrix_), whitened.matrix_); else {
typedef Eigen::Map<Eigen::MatrixXd> EigenMap;
typedef typeof(EigenMap(&jf.matrix_(0,0),0,0).block(0,0,0,0)) EigenBlock;
EigenBlock A(EigenMap(&jf.matrix_(0,0),jf.matrix_.size1(),jf.matrix_.size2()).block(
jf.Ab_.rowStart(),jf.Ab_.offset(0), jf.Ab_.full().size1(), jf.Ab_.full().size2()));
typedef typeof(Eigen::Map<Eigen::VectorXd>(&jf.model_->invsigmas()(0),0).asDiagonal()) EigenDiagonal;
EigenDiagonal R(Eigen::Map<Eigen::VectorXd>(&jf.model_->invsigmas()(0),jf.model_->dim()).asDiagonal());
info_.copyStructureFrom(jf.Ab_);
EigenMap L(EigenMap(&matrix_(0,0), matrix_.size1(), matrix_.size2()));
L.noalias() = A.transpose() * R * R * A;
}
} else if(dynamic_cast<const HessianFactor*>(&gf)) { } else if(dynamic_cast<const HessianFactor*>(&gf)) {
const HessianFactor& hf(static_cast<const HessianFactor&>(gf)); const HessianFactor& hf(static_cast<const HessianFactor&>(gf));
info_.assignNoalias(hf.info_); info_.assignNoalias(hf.info_);

6
gtsam/linear/NoiseModel.h
View File

@ -239,6 +239,12 @@ namespace gtsam {
inline const Vector& sigmas() const { return sigmas_; } inline const Vector& sigmas() const { return sigmas_; }
inline double sigma(size_t i) const { return sigmas_(i); } inline double sigma(size_t i) const { return sigmas_(i); }
/**
* Return sqrt precisions
*/
const Vector& invsigmas() const { return invsigmas_; }
double invsigma(size_t i) const { return invsigmas_(i); }
/** /**
* generate random variate * generate random variate
*/ */