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

Fix a bug in getb and replace it with negated values of gradientAtZero. Add some comments about a bug.

release/4.3a0
Sungtae An 2014-12-04 22:08:26 -05:00
parent f9d6c3da22
commit 60f43c7a4b
1 changed files with 44 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
gtsam/linear/PCGSolver.cpp
View File

@ -3,6 +3,7 @@
*
* Created on: Feb 14, 2012
* Author: ydjian
* Author: Sungtae An
*/
#include <gtsam/linear/GaussianFactorGraph.h>
@ -80,9 +81,13 @@ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const {
void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const {
/* implement A^t*A*x, assume x and AtAx are pre-allocated */
// Build a VectorValues for Vector x
VectorValues vvX = buildVectorValues(x,keyInfo_);
// VectorValues form of A'Ax for multiplyHessianAdd
VectorValues vvAtAx;
// vvAtAx += 1.0 * A'Ax for each factor
gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx);
// Make the result as Vector form
AtAx = vvAtAx.vector();
}
@ -91,30 +96,46 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const {
void GaussianFactorGraphSystem::getb(Vector &b) const {
/* compute rhs, assume b pre-allocated */
/* reset */
b.setZero();
/* ------------------------------------------------------------------------
* Multiply and getb functions (build function in preconditioner.cpp also)
* Yong-Dian's code had a bug that they do not consider noise model
* which means that they do not whiten A and b.
* It has no problem when the associated noise model has a form of Isotropic
* because it can be cancelled out on both l.h.s and r.h.s of equation.
* However, it cause a wrong result with non-isotropic noise model.
* The unit test for PCSSolver (testPCGSolver.cpp) Yond-Dian made use a
* example factor graph which has isotropic noise model and
* that is the reason why there was no unit test error.
* ------------------------------------------------------------------------*/
// /* reset */
// b.setZero();
//
// BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) {
// if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
// const Vector rhs = jf->getb();
// /* accumulate At rhs */
// for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
// /* this map lookup should be replaced */
// const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
// b.segment(entry.colstart(), entry.dim()) += jf->getA(it).transpose() * rhs ;
// }
// }
// else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
// /* accumulate g */
// for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) {
// const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
// b.segment(entry.colstart(), entry.dim()) += hf->linearTerm(it);
// }
// }
// else {
// throw invalid_argument("GaussianFactorGraphSystem::getb gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
// }
// }
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) {
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
const Vector rhs = jf->getb();
/* accumulate At rhs */
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
/* this map lookup should be replaced */
const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
b.segment(entry.colstart(), entry.dim()) += jf->getA(it).transpose() * rhs ;
}
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
/* accumulate g */
for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) {
const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
b.segment(entry.colstart(), entry.dim()) += hf->linearTerm(it);
}
}
else {
throw invalid_argument("GaussianFactorGraphSystem::getb gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
}
}
// Get whitened r.h.s (b vector) from each factor in the form of VectorValues
VectorValues vvb = gfg_.gradientAtZero();
// Make the result as Vector form
b = -vvb.vector();
}
/**********************************************************************************/