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

Fix function 'multiply'

release/4.3a0
Sungtae An 2014-12-03 12:19:01 -05:00
parent c4841c6e21
commit 5029d84ce2
1 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/linear/PCGSolver.cpp
View File

@ -77,26 +77,31 @@ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const {
} }
/*****************************************************************************/ /*****************************************************************************/
void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& Ax) const { void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const {
/* implement Ax, assume x and Ax are pre-allocated */ /* implement A^t*A*x, assume x and AtAx are pre-allocated */
/* reset y */ /* reset y */
Ax.setZero(); AtAx.setZero();
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) {
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) { if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
/* accumulate At A x*/ /* accumulate At A x*/
Vector Ax = zero(jf->rows());
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
const Matrix Ai = jf->getA(it); const Matrix Ai = jf->getA(it);
/* this map lookup should be replaced */ /* this map lookup should be replaced */
const KeyInfoEntry &entry = keyInfo_.find(*it)->second; const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
Ax.segment(entry.colstart(), entry.dim()) Ax += (Ai * x.segment(entry.colstart(), entry.dim()));
+= Ai.transpose() * (Ai * x.segment(entry.colstart(), entry.dim())); }
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
const Matrix Ai = jf->getA(it);
/* this map lookup should be replaced */
const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
AtAx.segment(entry.colstart(), entry.dim()) += Ai.transpose()*Ax;
} }
} }
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) { else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
/* accumulate H x */ /* accumulate H x */
/* use buffer to avoid excessive table lookups */ /* use buffer to avoid excessive table lookups */
const size_t sz = hf->size(); const size_t sz = hf->size();
vector<Vector> y; vector<Vector> y;
@ -122,7 +127,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& Ax) const {
for(DenseIndex i = 0; i < (DenseIndex) sz; ++i) { for(DenseIndex i = 0; i < (DenseIndex) sz; ++i) {
/* retrieve the key mapping */ /* retrieve the key mapping */
const KeyInfoEntry &entry = keyInfo_.find(hf->keys()[i])->second; const KeyInfoEntry &entry = keyInfo_.find(hf->keys()[i])->second;
Ax.segment(entry.colstart(), entry.dim()) += y[i]; AtAx.segment(entry.colstart(), entry.dim()) += y[i];
} }
} }
else { else {