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

fix for cityslam

release/4.3a0
Yong-Dian Jian 2010-10-15 02:40:46 +00:00
parent d55e72ae53
commit fc66445be8
3 changed files with 42 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
linear/Makefile.am
View File

@ -31,6 +31,8 @@ check_PROGRAMS += tests/testGaussianFactor tests/testGaussianConditional
check_PROGRAMS += tests/testGaussianJunctionTree
# Iterative Methods
headers += iterative-inl.h
sources += iterative.cpp SubgraphPreconditioner.cpp
#headers += iterative-inl.h SubgraphSolver.h SubgraphSolver-inl.h
#sources += iterative.cpp BayesNetPreconditioner.cpp SubgraphPreconditioner.cpp
#check_PROGRAMS += tests/testBayesNetPreconditioner

53
linear/SubgraphPreconditioner.cpp
View File

@ -64,9 +64,8 @@ namespace gtsam {
// Add A2 contribution
VectorValues x = this->x(y);
Errors e2 = Ab2_->errors(x);
e.splice(e.end(), e2);
return 0.5 * dot(e, e);
return 0.5 * (dot(e, e) + dot(e2,e2));
}
/* ************************************************************************* */
@ -136,19 +135,26 @@ namespace gtsam {
// Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2
VectorValues SubgraphPreconditioner::operator^(const Errors& e) const {
VectorValues y;
// VectorValues y;
//
// // Use BayesNet order to remove y contributions in order
// Errors::const_iterator it = e.begin();
// BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) {
// const Symbol& j = cg->key();
// const Vector& ej = *(it++);
// y.insert(j,ej);
// }
//
// // get A2 part
// transposeMultiplyAdd2(1.0,it,e.end(),y);
//
// return y;
// Use BayesNet order to remove y contributions in order
Errors::const_iterator it = e.begin();
BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) {
const Symbol& j = cg->key();
const Vector& ej = *(it++);
y.insert(j,ej);
}
// get A2 part
VectorValues y = zero();
for ( int i = 0 ; i < y.size() ; ++i, ++it )
y[i] = *it ;
transposeMultiplyAdd2(1.0,it,e.end(),y);
return y;
}
@ -157,15 +163,22 @@ namespace gtsam {
void SubgraphPreconditioner::transposeMultiplyAdd
(double alpha, const Errors& e, VectorValues& y) const {
// Use BayesNet order to remove y contributions in order
Errors::const_iterator it = e.begin();
BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) {
const Symbol& j = cg->key();
const Vector& ej = *(it++);
axpy(alpha,ej,y[j]);
}
// // Use BayesNet order to remove y contributions in order
// Errors::const_iterator it = e.begin();
// BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) {
// const Symbol& j = cg->key();
// const Vector& ej = *(it++);
// axpy(alpha,ej,y[j]);
// }
//
// // get A2 part
// transposeMultiplyAdd2(alpha,it,e.end(),y);
// get A2 part
Errors::const_iterator it = e.begin();
for ( int i = 0 ; i < y.size() ; ++i, ++it ) {
const Vector& ei = *it;
axpy(alpha,ei,y[i]) ;
}
transposeMultiplyAdd2(alpha,it,e.end(),y);
}

14
linear/SubgraphPreconditioner.h
View File

@ -55,13 +55,13 @@ namespace gtsam {
*/
SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2, sharedBayesNet& Rc1, sharedValues& xbar);
std::pair<Matrix,Vector> Ab1(const Ordering& ordering) const { return Ab1_->matrix(ordering); }
std::pair<Matrix,Vector> Ab2(const Ordering& ordering) const { return Ab2_->matrix(ordering); }
Matrix A1(const Ordering& ordering) const { return Ab1_->sparse(ordering); }
Matrix A2(const Ordering& ordering) const { return Ab2_->sparse(Ab1_->columnIndices(ordering)); }
Vector b1() const { return Ab1_->rhsVector(); }
Vector b2() const { return Ab2_->rhsVector(); }
VectorValues assembleValues(const Vector& v, const Ordering& ordering) const { return Ab1_->assembleValues(v, ordering); }
// std::pair<Matrix,Vector> Ab1(const Ordering& ordering) const { return Ab1_->matrix(ordering); }
// std::pair<Matrix,Vector> Ab2(const Ordering& ordering) const { return Ab2_->matrix(ordering); }
// Matrix A1(const Ordering& ordering) const { return Ab1_->sparse(ordering); }
// Matrix A2(const Ordering& ordering) const { return Ab2_->sparse(Ab1_->columnIndices(ordering)); }
// Vector b1() const { return Ab1_->rhsVector(); }
// Vector b2() const { return Ab2_->rhsVector(); }
// VectorValues assembleValues(const Vector& v, const Ordering& ordering) const { return Ab1_->assembleValues(v, ordering); }
/* x = xbar + inv(R1)*y */
VectorValues x(const VectorValues& y) const;