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

updates for cityslam, unfinished..

release/4.3a0
Yong-Dian Jian 2010-10-14 16:08:16 +00:00
parent ea7a67c435
commit ae0e51aaff
7 changed files with 106 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
linear/Errors.cpp
View File

@ -25,6 +25,18 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
Errors::Errors(){}
/* ************************************************************************* */
Errors::Errors(const VectorValues &V) {
this->resize(V.size()) ;
int i = 0 ;
BOOST_FOREACH( Vector &e, *this) {
e = V[i++];
}
}
/* ************************************************************************* */
void Errors::print(const std::string& s) const {
odprintf("%s:\n", s.c_str());
@ -47,6 +59,21 @@ bool Errors::equals(const Errors& expected, double tol) const {
// TODO: use boost::bind(&equal_with_abs_tol,_1, _2,tol)
}
/* ************************************************************************* */
Errors Errors::operator+(const Errors& b) const {
#ifndef NDEBUG
size_t m = size();
if (b.size()!=m)
throw(std::invalid_argument("Errors::operator+: incompatible sizes"));
#endif
Errors result;
Errors::const_iterator it = b.begin();
BOOST_FOREACH(const Vector& ai, *this)
result.push_back(ai + *(it++));
return result;
}
/* ************************************************************************* */
Errors Errors::operator-(const Errors& b) const {
#ifndef NDEBUG

10
linear/Errors.h
View File

@ -23,6 +23,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/Vector.h>
#include <gtsam/linear/VectorValues.h>
namespace gtsam {
@ -31,15 +32,24 @@ namespace gtsam {
public:
Errors() ;
/** break V into pieces according to its start indices */
Errors(const VectorValues &V) ;
/** print */
void print(const std::string& s = "Errors") const;
/** equals, for unit testing */
bool equals(const Errors& expected, double tol=1e-9) const;
/** Addition */
Errors operator+(const Errors& b) const;
/** subtraction */
Errors operator-(const Errors& b) const;
}; // Errors
/**

14
linear/GaussianFactorGraph.cpp
View File

@ -127,13 +127,13 @@ void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
Ai->transposeMultiplyAdd(alpha,*(ei++),x);
}
///* ************************************************************************* */
//VectorValues GaussianFactorGraph::gradient(const VectorValues& x) const {
// // It is crucial for performance to make a zero-valued clone of x
// VectorValues g = VectorValues::zero(x);
// transposeMultiplyAdd(1.0, errors(x), g);
// return g;
//}
/* ************************************************************************* */
VectorValues GaussianFactorGraph::gradient(const VectorValues& x) const {
// It is crucial for performance to make a zero-valued clone of x
VectorValues g = VectorValues::zero(x);
transposeMultiplyAdd(1.0, errors(x), g);
return g;
}
///* ************************************************************************* */
//set<Index> GaussianFactorGraph::find_separator(Index key) const

12
linear/GaussianFactorGraph.h
View File

@ -116,12 +116,12 @@ namespace gtsam {
/** x += alpha*A'*e */
void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const;
// /**
// * Calculate Gradient of A^(A*x-b) for a given config
// * @param x: VectorValues specifying where to calculate gradient
// * @return gradient, as a VectorValues as well
// */
// VectorValues gradient(const VectorValues& x) const;
/**
* Calculate Gradient of A^(A*x-b) for a given config
* @param x: VectorValues specifying where to calculate gradient
* @return gradient, as a VectorValues as well
*/
VectorValues gradient(const VectorValues& x) const;
/** Unnormalized probability. O(n) */
double probPrime(const VectorValues& c) const {

57
linear/SubgraphPreconditioner.cpp
View File

@ -44,13 +44,22 @@ namespace gtsam {
/* ************************************************************************* */
double SubgraphPreconditioner::error(const VectorValues& y) const {
Errors e;
// Errors e;
//
// // Use BayesNet order to add y contributions in order
// BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) {
// const Symbol& j = cg->key();
// e.push_back(y[j]); // append y
// }
//
// // Add A2 contribution
// VectorValues x = this->x(y);
// Errors e2 = Ab2_->errors(x);
// e.splice(e.end(), e2);
//
// return 0.5 * dot(e, e);
// Use BayesNet order to add y contributions in order
BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) {
const Symbol& j = cg->key();
e.push_back(y[j]); // append y
}
Errors e(y);
// Add A2 contribution
VectorValues x = this->x(y);
@ -75,13 +84,15 @@ namespace gtsam {
// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
Errors e;
// Errors e;
//
// // Use BayesNet order to add y contributions in order
// BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) {
// const Symbol& j = cg->key();
// e.push_back(y[j]); // append y
// }
// Use BayesNet order to add y contributions in order
BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) {
const Symbol& j = cg->key();
e.push_back(y[j]); // append y
}
Errors e(y);
// Add A2 contribution
VectorValues x = y; // TODO avoid ?
@ -96,13 +107,23 @@ namespace gtsam {
// In-place version that overwrites e
void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
Errors::iterator ei = e.begin();
// Errors::iterator ei = e.begin();
//
// // Use BayesNet order to add y contributions in order
// BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) {
// const Symbol& j = cg->key();
// *ei = y[j]; // append y
// ei++;
// }
//
// // Add A2 contribution
// VectorValues x = y; // TODO avoid ?
// gtsam::backSubstituteInPlace(*Rc1_, x); // x=inv(R1)*y
// Ab2_->multiplyInPlace(x,ei); // use iterator version
// Use BayesNet order to add y contributions in order
BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) {
const Symbol& j = cg->key();
*ei = y[j]; // append y
ei++;
Errors::iterator ei = e.begin();
for ( int i = 0 ; i < y.size() ; ++i, ++ei ) {
*ei = y[i];
}
// Add A2 contribution

16
linear/VectorValues.h
View File

@ -145,10 +145,17 @@ public:
VectorValues result;
result.varStarts_ = varStarts_;
result.values_ = boost::numeric::ublas::project(values_, boost::numeric::ublas::range(0, varStarts_.back())) +
boost::numeric::ublas::project(c.values_, boost::numeric::ublas::range(0, c.varStarts_.back()));
boost::numeric::ublas::project(c.values_, boost::numeric::ublas::range(0, c.varStarts_.back()));
return result;
}
void operator+=(const VectorValues& c) {
assert(varStarts_ == c.varStarts_);
this->values_ = boost::numeric::ublas::project(this->values_, boost::numeric::ublas::range(0, varStarts_.back())) +
boost::numeric::ublas::project(c.values_, boost::numeric::ublas::range(0, c.varStarts_.back()));
}
/**
* Iterator (handles both iterator and const_iterator depending on whether
* the template type is const.
@ -177,11 +184,18 @@ public:
value_type operator*() { return config_[curVariable_]; }
};
static VectorValues zero(const VectorValues& x) {
VectorValues cloned(x);
cloned.makeZero();
return cloned;
}
protected:
void checkVariable(Index variable) const { assert(variable < varStarts_.size()-1); }
public:
friend size_t dim(const VectorValues& V) { return V.varStarts_.back(); }
friend double dot(const VectorValues& V1, const VectorValues& V2) { return gtsam::dot(V1.values_, V2.values_) ; }
friend void scal(double alpha, VectorValues& x) { gtsam::scal(alpha, x.values_) ; }
friend void axpy(double alpha, const VectorValues& x, VectorValues& y) { gtsam::axpy(alpha, x.values_, y.values_) ; }

4
linear/iterative-inl.h
View File

@ -43,8 +43,8 @@ namespace gtsam {
k = 0;
verbose = verb;
steepest = steep;
maxIterations = (maxIt > 0) ? maxIt : x.dim() * (steepest ? 10 : 1);
reset = (size_t) (sqrt(x.dim()) + 0.5); // when to reset
maxIterations = (maxIt > 0) ? maxIt : dim(x) * (steepest ? 10 : 1);
reset = (size_t) (sqrt(dim(x)) + 0.5); // when to reset
// Start with g0 = A'*(A*x0-b), d0 = - g0
// i.e., first step is in direction of negative gradient