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-13 22:41:26 +00:00
parent 47b11749fe
commit de90c84d91
3 changed files with 21 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
linear/SubgraphPreconditioner.h
View File

@ -8,7 +8,7 @@
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/inference/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
namespace gtsam { namespace gtsam {
@ -56,7 +56,11 @@ namespace gtsam {
VectorValues x(const VectorValues& y) const; VectorValues x(const VectorValues& y) const;
/* A zero VectorValues with the structure of xbar */ /* A zero VectorValues with the structure of xbar */
VectorValues zero() const { return VectorValues::zero(*xbar_);} VectorValues zero() const {
VectorValues V(*xbar_) ;
V.makeZero();
return V ;
}
/* error, given y */ /* error, given y */
double error(const VectorValues& y) const; double error(const VectorValues& y) const;

12
linear/VectorValues.h
View File

@ -46,6 +46,7 @@ public:
* slow reallocation of space at runtime. * slow reallocation of space at runtime.
*/ */
VectorValues() : varStarts_(1,0) {} VectorValues() : varStarts_(1,0) {}
VectorValues(const VectorValues &V) : values_(V.values_), varStarts_(V.varStarts_) {}
/** Construct from a container of variable dimensions (in variable order). */ /** Construct from a container of variable dimensions (in variable order). */
template<class Container> template<class Container>
@ -76,6 +77,10 @@ public:
*/ */
size_t dim() const { return varStarts_.back(); } size_t dim() const { return varStarts_.back(); }
/* dot product */
double dot(const VectorValues& V) const { return gtsam::dot(this->values_, V.values_) ; }
/** Total dimensions capacity allocated */ /** Total dimensions capacity allocated */
size_t dimCapacity() const { return values_.size(); } size_t dimCapacity() const { return values_.size(); }
@ -163,6 +168,13 @@ public:
protected: protected:
void checkVariable(Index variable) const { assert(variable < varStarts_.size()-1); } void checkVariable(Index variable) const { assert(variable < varStarts_.size()-1); }
public:
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

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