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

Major check-in: there are now two interchangeable implementations of VectorConfig. 

VectorMap uses a straightforward stl::map of Vectors. It has O(log n)
insert and access, and is fairly fast at both. However, it has high overhead
for arithmetic operations such as +, scale, axpy etc...

VectorBTree uses a functional BTree as a way to access SubVectors
in an ordinary Vector. Inserting is O(n) and much slower, but accessing,
is O(log n) and might be a bit slower than VectorMap. Arithmetic operations
are blindingly fast, however. The cost is it is not as KISS as VectorMap.

Access to vectors is now exclusively via operator[]
Vector access in VectorMap is via a Vector reference
Vector access in VectorBtree is via the SubVector type (see Vector.h)

Feb 16 2010: FD: I made VectorMap the default, because I decided to try
and speed up conjugate gradients by using Sparse FactorGraphs all the way.
release/4.3a0
Frank Dellaert 2010-02-17 03:29:12 +00:00
parent cb5d4c3127
commit 3247751b5d
37 changed files with 1385 additions and 572 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
.cproject
View File

@ -20,7 +20,7 @@
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.MachO;org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder arguments="" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="2" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<builder arguments="" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="false" parallelizationNumber="2" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">
@ -469,6 +469,7 @@
</target>
<target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -476,7 +477,6 @@
</target>
<target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -484,6 +484,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -699,6 +700,7 @@
</target>
<target name="testGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -754,7 +756,6 @@
</target>
<target name="testSimulated2D.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -792,6 +793,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

2
.project
View File

@ -23,7 +23,7 @@
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildArguments</key>
<value>-j2</value>
<value></value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildCommand</key>

36
cpp/BTree.h
View File

@ -29,9 +29,9 @@ namespace gtsam {
*/
struct Node {
size_t height_;
const size_t height_;
const value_type keyValue_;
BTree left, right;
const BTree left, right;
/** default constructor */
Node() {
@ -48,9 +48,8 @@ namespace gtsam {
* Create a node from two subtrees and a key value pair
*/
Node(const BTree& l, const value_type& keyValue, const BTree& r) :
left(l), keyValue_(keyValue), right(r) {
size_t hl = l.height(), hr = r.height();
height_ = hl >= hr ? hl + 1 : hr + 1;
left(l), keyValue_(keyValue), right(r),
height_(l.height() >= r.height() ? l.height() + 1 : r.height() + 1) {
}
inline const Key& key() const { return keyValue_.first;}
@ -60,7 +59,7 @@ namespace gtsam {
// We store a shared pointer to the root of the functional tree
// composed of Node classes. If root_==NULL, the tree is empty.
typedef boost::shared_ptr<Node> sharedNode;
typedef boost::shared_ptr<const Node> sharedNode;
sharedNode root_;
inline const value_type& keyValue() const { return root_->keyValue_;}
@ -100,6 +99,11 @@ namespace gtsam {
BTree() {
}
/** copy constructor */
BTree(const BTree& other) :
root_(other.root_) {
}
/** create leaf from key-value pair */
BTree(const value_type& keyValue) :
root_(new Node(keyValue)) {
@ -208,25 +212,12 @@ namespace gtsam {
return left().size() + 1 + right().size();
}
#ifdef RECURSIVE_FIND
/** find a value given a key, throws exception when not found */
const Value& find(const Key& k) const {
if (!root_) throw std::invalid_argument("BTree::find: key '"
+ (std::string) k + "' not found");
const Node& node = *root_;
const Key& key = node.key();
if (k < key) return node.left.find(k);
if (key < k) return node.right.find(k);
return node.value(); // (key() == k)
}
#else
/**
* find a value given a key, throws exception when not found
* Optimized non-recursive version as [find] is crucial for speed
*/
const Value& find(const Key& k) const {
Node* node = root_.get();
const Node* node = root_.get();
while (node) {
const Key& key = node->key();
if (k < key) node = node->left.root_.get();
@ -235,7 +226,6 @@ namespace gtsam {
}
throw std::invalid_argument("BTree::find: key '" + (std::string) k + "' not found");
}
#endif
/** print in-order */
void print(const std::string& s = "") const {
@ -317,7 +307,7 @@ namespace gtsam {
path_.top().second = true; // flag we visited right
// push right root and its left-most path onto the stack
while (t) {
path_.push(make_pair(t, false));
path_.push(std::make_pair(t, false));
t = t->left.root_;
}
}
@ -340,7 +330,7 @@ namespace gtsam {
const_iterator(const sharedNode& root) {
sharedNode t = root;
while (t) {
path_.push(make_pair(t, false));
path_.push(std::make_pair(t, false));
t = t->left.root_;
}
}

9
cpp/BayesNetPreconditioner.cpp
View File

@ -36,7 +36,9 @@ namespace gtsam {
/* ************************************************************************* */
// gradient is inv(R')*A'*(A*inv(R)*y-b),
VectorConfig BayesNetPreconditioner::gradient(const VectorConfig& y) const {
VectorConfig gx = Ab_ ^ Ab_.errors(x(y));
VectorConfig gx = VectorConfig::zero(y);
Errors e = Ab_.errors(x(y));
Ab_.transposeMultiplyAdd(1.0,e,gx);
return gtsam::backSubstituteTranspose(Rd_, gx);
}
@ -66,8 +68,9 @@ namespace gtsam {
// y += alpha*inv(R')*A'*e
void BayesNetPreconditioner::transposeMultiplyAdd(double alpha,
const Errors& e, VectorConfig& y) const {
VectorConfig x = Ab_ ^ e; // x = A'*e2
y += alpha * gtsam::backSubstituteTranspose(Rd_, x); // TODO avoid temp
VectorConfig x = VectorConfig::zero(y);
Ab_.transposeMultiplyAdd(1.0,e,x); // x += A'*e
axpy(alpha, gtsam::backSubstituteTranspose(Rd_, x), y); // y += alpha*inv(R')*x
}
/* ************************************************************************* */

4
cpp/BayesTree-inl.h
View File

@ -60,7 +60,7 @@ namespace gtsam {
template<class Conditional>
size_t BayesTree<Conditional>::Clique::treeSize() const {
size_t size = 1;
BOOST_FOREACH(shared_ptr child, children_)
BOOST_FOREACH(const shared_ptr& child, children_)
size += child->treeSize();
return size;
}
@ -69,7 +69,7 @@ namespace gtsam {
template<class Conditional>
void BayesTree<Conditional>::Clique::printTree(const string& indent) const {
print(indent);
BOOST_FOREACH(shared_ptr child, children_)
BOOST_FOREACH(const shared_ptr& child, children_)
child->printTree(indent+" ");
}

23
cpp/GaussianBayesNet.cpp
View File

@ -98,11 +98,9 @@ void backSubstituteInPlace(const GaussianBayesNet& bn, VectorConfig& y) {
for (it = cg->parentsBegin(); it!= cg->parentsEnd(); it++) {
const Symbol& j = it->first;
const Matrix& Rij = it->second;
Vector& xj = x.getReference(j);
multiplyAdd(-1.0,Rij,xj,zi);
multiplyAdd(-1.0,Rij,x[j],zi);
}
Vector& xi = x.getReference(i);
xi = gtsam::backSubstituteUpper(cg->get_R(), zi);
x[i] = gtsam::backSubstituteUpper(cg->get_R(), zi);
}
}
@ -114,33 +112,26 @@ VectorConfig backSubstituteTranspose(const GaussianBayesNet& bn,
const VectorConfig& gx) {
// Initialize gy from gx
VectorConfig gy;
BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn) {
const Symbol& j = cg->key();
Vector gyj = gx.contains(j) ? gx[j] : zero(cg->dim());
gy.insert(j,gyj); // initialize result
}
// TODO: used to insert zeros if gx did not have an entry for a variable in bn
VectorConfig gy = gx;
// we loop from first-eliminated to last-eliminated
// i^th part of L*gy=gx is done block-column by block-column of L
BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn) {
const Symbol& j = cg->key();
Vector& gyj = gy.getReference(j); // should never fail
gyj = gtsam::backSubstituteUpper(gyj,cg->get_R());
gy[j] = gtsam::backSubstituteUpper(gy[j],cg->get_R());
GaussianConditional::const_iterator it;
for (it = cg->parentsBegin(); it!= cg->parentsEnd(); it++) {
const Symbol& i = it->first;
const Matrix& Rij = it->second;
Vector& gyi = gy.getReference(i); // should never fail
transposeMultiplyAdd(-1.0,Rij,gyj,gyi);
transposeMultiplyAdd(-1.0,Rij,gy[j],gy[i]);
}
}
// Scale gy
BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn) {
const Symbol& j = cg->key();
Vector& gyj = gy.getReference(j); // should never fail
gyj = emul(gyj,cg->get_sigmas());
gy[j] = emul(gy[j],cg->get_sigmas());
}
return gy;

27
cpp/GaussianFactor.cpp
View File

@ -215,10 +215,7 @@ void GaussianFactor::transposeMultiplyAdd(double alpha, const Vector& e,
Vector E = alpha * model_->whiten(e);
// Just iterate over all A matrices and insert Ai^e into VectorConfig
FOREACH_PAIR(j, Aj, As_)
{
Vector& Xj = x.getReference(*j);
gtsam::transposeMultiplyAdd(1.0, *Aj, E, Xj);
}
gtsam::transposeMultiplyAdd(1.0, *Aj, E, x[*j]);
}
/* ************************************************************************* */
@ -481,28 +478,6 @@ GaussianFactor::eliminate(const Symbol& key) const
return eliminateMatrix(Ab, model_, ordering, dimensions());
}
/* ************************************************************************* */
// Creates a factor on step-size, given initial estimate and direction d, e.g.
// Factor |A1*x+A2*y-b|/sigma -> |A1*(x0+alpha*dx)+A2*(y0+alpha*dy)-b|/sigma
// -> |(A1*dx+A2*dy)*alpha-(b-A1*x0-A2*y0)|/sigma
/* ************************************************************************* */
GaussianFactor::shared_ptr GaussianFactor::alphaFactor(const Symbol& key, const VectorConfig& x,
const VectorConfig& d) const {
// Calculate A matrix
size_t m = b_.size();
Vector A = zero(m);
FOREACH_PAIR(j, Aj, As_)
A += *Aj * d[*j];
// calculate the value of the factor for RHS
Vector b = - unweighted_error(x);
// construct factor
shared_ptr factor(new GaussianFactor(key,Matrix_(A),b,model_));
return factor;
}
/* ************************************************************************* */
namespace gtsam {

7
cpp/GaussianFactor.h
View File

@ -230,13 +230,6 @@ public:
boost::tuple<std::list<int>, std::list<int>, std::list<double> >
sparse(const Dimensions& columnIndices) const;
/**
* Create a GaussianFactor on one variable 'alpha' (step size), in direction d
* @param x: starting point for search
* @param d: search direction
*/
shared_ptr alphaFactor(const Symbol& key, const VectorConfig& x, const VectorConfig& d) const;
/* ************************************************************************* */
// MUTABLE functions. FD:on the path to being eradicated
/* ************************************************************************* */

33
cpp/GaussianFactorGraph.cpp
View File

@ -86,8 +86,10 @@ VectorConfig GaussianFactorGraph::operator^(const Errors& e) const {
VectorConfig x;
// For each factor add the gradient contribution
Errors::const_iterator it = e.begin();
BOOST_FOREACH(sharedFactor Ai,factors_)
x += (*Ai)^(*(it++));
BOOST_FOREACH(sharedFactor Ai,factors_) {
VectorConfig xi = (*Ai)^(*(it++));
x.insertAdd(xi);
}
return x;
}
@ -103,8 +105,10 @@ void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
/* ************************************************************************* */
VectorConfig GaussianFactorGraph::gradient(const VectorConfig& x) const {
const GaussianFactorGraph& A = *this;
return A^errors(x);
// It is crucial for performance to make a zero-valued clone of x
VectorConfig g = VectorConfig::zero(x);
transposeMultiplyAdd(1.0, errors(x), g);
return g;
}
/* ************************************************************************* */
@ -389,27 +393,6 @@ Matrix GaussianFactorGraph::sparse(const Dimensions& indices) const {
return ijs;
}
/* ************************************************************************* */
VectorConfig GaussianFactorGraph::optimalUpdate(const VectorConfig& x,
const VectorConfig& d) const {
// create a new graph on step-size
GaussianFactorGraph alphaGraph;
Symbol alphaKey('\224', 1);
BOOST_FOREACH(sharedFactor factor,factors_) {
sharedFactor alphaFactor = factor->alphaFactor(alphaKey, x,d);
alphaGraph.push_back(alphaFactor);
}
// solve it for optimal step-size alpha
GaussianConditional::shared_ptr gc = alphaGraph.eliminateOne(alphaKey);
double alpha = gc->get_d()(0);
cout << alpha << endl;
// return updated estimate by stepping in direction d
return expmap(x, d.scale(alpha));
}
/* ************************************************************************* */
VectorConfig GaussianFactorGraph::steepestDescent(const VectorConfig& x0,
bool verbose, double epsilon, size_t maxIterations) const {

8
cpp/GaussianFactorGraph.h
View File

@ -224,14 +224,6 @@ namespace gtsam {
*/
Matrix sparse(const Dimensions& indices) const;
/**
* Take an optimal step in direction d by calculating optimal step-size
* @param x: starting point for search
* @param d: search direction
*/
VectorConfig optimalUpdate(const VectorConfig& x0, const VectorConfig& d) const;
/**
* Find solution using gradient descent
* @param x0: VectorConfig specifying initial estimate

2
cpp/GaussianISAM.cpp
View File

@ -24,7 +24,7 @@ void optimize(const GaussianISAM::sharedClique& clique, VectorConfig& result) {
Vector x = cg->solve(result); // Solve for that variable
result.insert(cg->key(), x); // store result in partial solution
}
BOOST_FOREACH(GaussianISAM::sharedClique child, clique->children_) {
BOOST_FOREACH(const GaussianISAM::sharedClique& child, clique->children_) {
// list<GaussianISAM::Clique::shared_ptr>::const_iterator child;
// for (child = clique->children_.begin(); child != clique->children_.end(); child++) {
optimize(child, result);

2
cpp/GaussianISAM2.cpp
View File

@ -31,7 +31,7 @@ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, Vect
result.insert(cg->key(), x); // store result in partial solution
}
if (process_children) {
BOOST_FOREACH(GaussianISAM2::sharedClique child, clique->children_) {
BOOST_FOREACH(const GaussianISAM2::sharedClique& child, clique->children_) {
optimize2(child, threshold, result);
}
}

1
cpp/LieConfig.h
View File

@ -13,6 +13,7 @@
#include "Vector.h"
#include "Testable.h"
#include "VectorConfig.h"
namespace boost { template<class T> class optional; }
namespace gtsam { class VectorConfig; }

20
cpp/Makefile.am
View File

@ -40,7 +40,13 @@ testMatrix_LDADD = libgtsam.la
# The header files will be installed in ~/include/gtsam
headers = gtsam.h Value.h Testable.h Factor.h Conditional.h
headers += Ordering.h IndexTable.h numericalDerivative.h
headers += BTree.h
sources += Ordering.cpp smallExample.cpp
check_PROGRAMS += testOrdering testBTree
testOrdering_SOURCES = testOrdering.cpp
testOrdering_LDADD = libgtsam.la
testBTree_SOURCES = testBTree.cpp
testBTree_LDADD = libgtsam.la
# Symbolic Inference
headers += SymbolicConditional.h
@ -70,8 +76,6 @@ testFactorgraph_SOURCES = testFactorgraph.cpp
testInference_SOURCES = testInference.cpp
testFactorgraph_LDADD = libgtsam.la
testInference_LDADD = libgtsam.la
testOrdering_SOURCES = testOrdering.cpp
testOrdering_LDADD = libgtsam.la
testBayesTree_SOURCES = testBayesTree.cpp
testBayesTree_LDADD = libgtsam.la
testGaussianISAM_SOURCES = testGaussianISAM.cpp
@ -88,11 +92,13 @@ testBinaryBayesNet_SOURCES = testBinaryBayesNet.cpp
testBinaryBayesNet_LDADD = libgtsam.la
# Gaussian inference
headers += GaussianFactorSet.h SharedGaussian.h SharedDiagonal.h
sources += NoiseModel.cpp Errors.cpp VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp
check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testNoiseModel
testVectorConfig_SOURCES = testVectorConfig.cpp
testVectorConfig_LDADD = libgtsam.la
headers += GaussianFactorSet.h SharedGaussian.h SharedDiagonal.h VectorConfig.h
sources += NoiseModel.cpp Errors.cpp VectorMap.cpp VectorBTree.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp
check_PROGRAMS += testVectorMap testVectorBTree testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testNoiseModel
testVectorMap_SOURCES = testVectorMap.cpp
testVectorMap_LDADD = libgtsam.la
testVectorBTree_SOURCES = testVectorBTree.cpp
testVectorBTree_LDADD = libgtsam.la
testGaussianFactor_SOURCES = testGaussianFactor.cpp
testGaussianFactor_LDADD = libgtsam.la
testGaussianFactorGraph_SOURCES = testGaussianFactorGraph.cpp

2
cpp/Matrix.cpp
View File

@ -187,7 +187,7 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector
}
/* ************************************************************************* */
void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector& x) {
void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) {
// ublas x += prod(trans(A),e) is terribly slow
// TODO: use BLAS
size_t m = A.size1(), n = A.size2();

2
cpp/Matrix.h
View File

@ -105,7 +105,7 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector
/**
* BLAS Level-2 style x <- x + alpha*A'*e
*/
void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector& x);
void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x);
/**
* overload * for vector*matrix multiplication (as BOOST does not)

8
cpp/Pose3.cpp
View File

@ -81,7 +81,7 @@ namespace gtsam {
const Matrix R = R_.matrix(), T = Matrix_(3,1, t_.vector());
const Matrix A34 = collect(2, &R, &T);
const Matrix A14 = Matrix_(1,4, 0.0, 0.0, 0.0, 1.0);
return stack(2, &A34, &A14);
return gtsam::stack(2, &A34, &A14);
}
/* ************************************************************************* */
@ -140,7 +140,7 @@ namespace gtsam {
Matrix DR_t1 = zeros(3, 3);
Matrix DR = collect(2, &DR_R1, &DR_t1);
Matrix Dt = Dtransform_from1(p1, p2.translation());
return stack(2, &DR, &Dt);
return gtsam::stack(2, &DR, &Dt);
}
Matrix Dcompose2(const Pose3& p1, const Pose3& p2) {
@ -149,7 +149,7 @@ namespace gtsam {
const static Matrix Z3 = zeros(3, 3);
Matrix DR = collect(2, &I, &Z3);
Matrix Dt = collect(2, &Z3, &R1);
return stack(2, &DR, &Dt);
return gtsam::stack(2, &DR, &Dt);
}
/* ************************************************************************* */
@ -162,7 +162,7 @@ namespace gtsam {
Matrix Dt_R1 = -skewSymmetric(unrotate(p.rotation(),p.translation()).vector());
Matrix Dt_t1 = -Rt;
Matrix Dt = collect(2, &Dt_R1, &Dt_t1);
return stack(2, &DR, &Dt);
return gtsam::stack(2, &DR, &Dt);
}
/* ************************************************************************* */

4
cpp/SubgraphPreconditioner-inl.h
View File

@ -68,9 +68,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class G, class T>
VectorConfig SubgraphPCG<G, T>::optimize(SubgraphPreconditioner& system) const {
//TODO: 3 is hard coded here
VectorConfig zeros;
BOOST_FOREACH(const Symbol& j, *ordering_) zeros.insert(j,zero(Pose::dim()));
VectorConfig zeros = system.zero();
// Solve the subgraph PCG
VectorConfig ybar = conjugateGradients<SubgraphPreconditioner, VectorConfig,

47
cpp/SubgraphPreconditioner.cpp
View File

@ -12,13 +12,18 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2, sharedBayesNet& Rc1, sharedConfig& xbar) :
SubgraphPreconditioner::SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2,
sharedBayesNet& Rc1, sharedConfig& xbar) :
Ab1_(Ab1), Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(Ab2_->errors_(*xbar)) {
}
/* ************************************************************************* */
// x = xbar + inv(R1)*y
VectorConfig SubgraphPreconditioner::x(const VectorConfig& y) const {
#ifdef VECTORBTREE
if (!y.cloned(*xbar_)) throw
invalid_argument("SubgraphPreconditioner::x: y needs to be cloned from xbar");
#endif
VectorConfig x = y;
backSubstituteInPlace(*Rc1_,x);
x += *xbar_;
@ -48,7 +53,9 @@ namespace gtsam {
// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
VectorConfig SubgraphPreconditioner::gradient(const VectorConfig& y) const {
VectorConfig x = this->x(y); // x = inv(R1)*y
VectorConfig gx2 = *Ab2_ ^ Ab2_->errors(x);
Errors e2 = Ab2_->errors(x);
VectorConfig gx2 = VectorConfig::zero(y);
Ab2_->transposeMultiplyAdd(1.0,e2,gx2); // A2'*e2;
VectorConfig gy2 = gtsam::backSubstituteTranspose(*Rc1_, gx2); // inv(R1')*gx2
return y + gy2;
}
@ -107,21 +114,14 @@ namespace gtsam {
y.insert(j,ej);
}
// create e2 with what's left of e
Errors e2;
while (it != e.end())
e2.push_back(*(it++));
// get A2 part,
VectorConfig x = *Ab2_ ^ e2; // x = A2'*e2
y += gtsam::backSubstituteTranspose(*Rc1_, x); // inv(R1')*x;
// get A2 part
transposeMultiplyAdd2(1.0,it,e.end(),y);
return y;
}
/* ************************************************************************* */
// y += alpha*A'*e
// TODO avoid code duplication
void SubgraphPreconditioner::transposeMultiplyAdd
(double alpha, const Errors& e, VectorConfig& y) const {
@ -130,18 +130,31 @@ namespace gtsam {
BOOST_FOREACH(GaussianConditional::shared_ptr cg, *Rc1_) {
const Symbol& j = cg->key();
const Vector& ej = *(it++);
Vector& yj = y.getReference(j);
axpy(alpha,ej,yj);
axpy(alpha,ej,y[j]);
}
// get A2 part
transposeMultiplyAdd2(alpha,it,e.end(),y);
}
/* ************************************************************************* */
// y += alpha*inv(R1')*A2'*e2
void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
Errors::const_iterator it, Errors::const_iterator end, VectorConfig& y) const {
// create e2 with what's left of e
// TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ?
Errors e2;
while (it != e.end())
while (it != end)
e2.push_back(*(it++));
// get A2 part,
VectorConfig x = *Ab2_ ^ e2; // x = A2'*e2
y += alpha * gtsam::backSubstituteTranspose(*Rc1_, x); // inv(R1')*x;
// Old code:
// VectorConfig x = *Ab2_ ^ e2; // x = A2'*e2
// y += alpha * gtsam::backSubstituteTranspose(*Rc1_, x); // inv(R1')*x;
// New Code:
VectorConfig x = VectorConfig::zero(y); // x = 0
Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2
axpy(alpha, gtsam::backSubstituteTranspose(*Rc1_, x), y); // y += alpha*inv(R1')*x
}
/* ************************************************************************* */

18
cpp/SubgraphPreconditioner.h
View File

@ -55,6 +55,9 @@ namespace gtsam {
/* x = xbar + inv(R1)*y */
VectorConfig x(const VectorConfig& y) const;
/* A zero VectorConfig with the structure of xbar */
VectorConfig zero() const { return VectorConfig::zero(*xbar_);}
/* error, given y */
double error(const VectorConfig& y) const;
@ -70,10 +73,21 @@ namespace gtsam {
/** Apply operator A' */
VectorConfig operator^(const Errors& e) const;
/** y += alpha*A'*e */
/**
* Add A'*e to y
* y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2]
*/
void transposeMultiplyAdd(double alpha, const Errors& e, VectorConfig& y) const;
/** print the object */
/**
* Add constraint part of the error only, used in both calls above
* y += alpha*inv(R1')*A2'*e2
* Takes a range indicating e2 !!!!
*/
void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin,
Errors::const_iterator end, VectorConfig& y) const;
/** print the object */
void print(const std::string& s = "SubgraphPreconditioner") const;
};

20
cpp/Vector.cpp
View File

@ -153,6 +153,24 @@ namespace gtsam {
return false;
}
/* ************************************************************************* */
bool assert_equal(SubVector expected, SubVector actual, double tol) {
if (equal_with_abs_tol(expected,actual,tol)) return true;
cout << "not equal:" << endl;
print(expected, "expected");
print(actual, "actual");
return false;
}
/* ************************************************************************* */
bool assert_equal(ConstSubVector expected, ConstSubVector actual, double tol) {
if (equal_with_abs_tol(expected,actual,tol)) return true;
cout << "not equal:" << endl;
print(expected, "expected");
print(actual, "actual");
return false;
}
/* ************************************************************************* */
Vector sub(const Vector &v, size_t i1, size_t i2) {
size_t n = i2-i1;
@ -263,7 +281,7 @@ namespace gtsam {
}
/* ************************************************************************* */
void axpy(double alpha, const Vector& x, SubVector& y) {
void axpy(double alpha, const Vector& x, SubVector y) {
size_t n = x.size();
assert (y.size()==n);
for (size_t i = 0; i < n; i++)

13
cpp/Vector.h
View File

@ -20,6 +20,7 @@
typedef boost::numeric::ublas::vector<double> Vector;
#endif
typedef boost::numeric::ublas::vector_range<Vector> SubVector;
typedef boost::numeric::ublas::vector_range<const Vector> ConstSubVector;
namespace gtsam {
@ -132,6 +133,16 @@ inline bool equal(const Vector& vec1, const Vector& vec2) {
*/
bool assert_equal(const Vector& vec1, const Vector& vec2, double tol=1e-9);
/**
* Same, prints if error
* @param vec1 Vector
* @param vec2 Vector
* @param tol 1e-9
* @return bool
*/
bool assert_equal(SubVector vec1, SubVector vec2, double tol=1e-9);
bool assert_equal(ConstSubVector vec1, ConstSubVector vec2, double tol=1e-9);
/**
* extract subvector, slice semantics, i.e. range = [i1,i2[ excluding i2
* @param v Vector
@ -220,7 +231,7 @@ void scal(double alpha, Vector& x);
* BLAS Level 1 axpy: y <- alpha*x + y
*/
void axpy(double alpha, const Vector& x, Vector& y);
void axpy(double alpha, const Vector& x, SubVector& y);
void axpy(double alpha, const Vector& x, SubVector y);
/**
* Divide every element of a Vector into a scalar

208
cpp/VectorBTree.cpp Normal file
View File

@ -0,0 +1,208 @@
/**
* @file VectorConfig.cpp
* @brief Factor Graph Configuration
* @brief VectorConfig
* @author Frank Dellaert
*/
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include "VectorBTree.h"
using namespace std;
namespace gtsam {
/* ************************************************************************* */
// Check if two VectorConfigs are compatible, throw exception if not
static void check_compatible(const string& s, const VectorBTree& a,
const VectorBTree& b) {
if (!a.compatible(b))
throw invalid_argument(s + ": incompatible VectorBTrees");
}
/* ************************************************************************* */
// Check if two VectorBTrees are cloned, throw exception if not
// The 2 configs need exactly the same ranges tree
static void check_cloned(const string& s, const VectorBTree& a,
const VectorBTree& b) {
if (!a.cloned(b))
throw invalid_argument(s + ": only defined for cloned VectorBTrees");
}
/* ************************************************************************* */
void VectorBTree::print(const string& name) const {
odprintf("VectorBTree %s\n", name.c_str());
odprintf("size: %d\n", size_);
BOOST_FOREACH(const Pair& p, ranges_) {
const Range& r = p.second;
odprintf("%s: %d:%d", string(p.first).c_str(), r.start(), r.size());
gtsam::print(get(r));
}
}
/* ************************************************************************* */
bool VectorBTree::equals(const VectorBTree& expected, double tol) const {
BOOST_FOREACH(const Pair& p, ranges_) {
const Symbol& j = p.first;
if (!expected.contains(j)) return false;
if (!equal_with_abs_tol(expected[j], get(p.second), tol))
return false;
}
return true;
}
/* ************************************************************************* */
VectorBTree& VectorBTree::insert(const Symbol& j, const Vector& v) {
if (contains(j)) throw invalid_argument("VectorBTree::insert: key already exists");
// calculate new range from current dimensionality and dim(a)
size_t n1 = values_.size(), n2 = n1 + v.size();
ranges_ = ranges_.add(j,Range(n1,n2));
// resize vector and COPY a into it
values_.resize(n2,true);
std::copy(v.begin(),v.end(),values_.begin()+n1);
// increment size
++size_;
return *this;
}
/* ************************************************************************* */
VectorBTree& VectorBTree::insertAdd(const Symbol& j, const Vector& v) {
if (!contains(j)) return insert(j,v);
const Range& r = ranges_.find(j);
SubVector(values_,r) += v;
return *this;
}
/* ************************************************************************* */
void VectorBTree::insert(const VectorBTree& config) {
BOOST_FOREACH(const Pair& p, config.ranges_)
insert(p.first,config.get(p.second));
}
/* ************************************************************************* */
void VectorBTree::insertAdd(const VectorBTree& config) {
BOOST_FOREACH(const Pair& p, config.ranges_)
insertAdd(p.first,config.get(p.second));
}
/* ************************************************************************* */
std::vector<Symbol> VectorBTree::get_names() const {
std::vector<Symbol> names;
BOOST_FOREACH(const Pair& p, ranges_)
names.push_back(p.first);
return names;
}
/* ************************************************************************* */
SubVector VectorBTree::operator[](const Symbol& j) {
const Range& r = ranges_.find(j);
return SubVector(values_,r);
}
/* ************************************************************************* */
ConstSubVector VectorBTree::operator[](const Symbol& j) const {
const Range& r = ranges_.find(j);
return ConstSubVector(values_,r);
}
/* ************************************************************************* */
double VectorBTree::max() const {
double m = -std::numeric_limits<double>::infinity();
BOOST_FOREACH(const Pair& p, ranges_)
m = std::max(m, gtsam::max(get(p.second)));
return m;
}
/* ************************************************************************* */
VectorBTree VectorBTree::scale(double s) const {
VectorBTree scaled = *this;
scaled.values_ *= s;
return scaled;
}
/* ************************************************************************* */
VectorBTree VectorBTree::operator*(double s) const {
return scale(s);
}
/* ************************************************************************* */
VectorBTree VectorBTree::operator-() const {
VectorBTree result = *this;
result.values_ = - values_;
return result;
}
/* ************************************************************************* */
void VectorBTree::operator+=(const VectorBTree& b) {
check_compatible("VectorBTree:operator+=", *this, b);
values_ += b.values_;
}
/* ************************************************************************* */
VectorBTree VectorBTree::operator+(const VectorBTree& b) const {
check_compatible("VectorBTree:operator+", *this, b);
VectorBTree result = *this;
result += b;
return result;
}
/* ************************************************************************* */
void VectorBTree::operator-=(const VectorBTree& b) {
check_compatible("VectorBTree:operator-=", *this, b);
values_ -= b.values_;
}
/* ************************************************************************* */
VectorBTree VectorBTree::operator-(const VectorBTree& b) const {
VectorBTree result = *this;
result -= b;
return result;
}
/* ************************************************************************* */
double VectorBTree::dot(const VectorBTree& b) const {
check_compatible("VectorBTree:dot", *this, b);
return gtsam::dot(values_,b.values_);
}
/* ************************************************************************* */
VectorBTree& VectorBTree::zero() {
std::fill(values_.begin(), values_.end(), 0.0);
return *this;
}
/* ************************************************************************* */
VectorBTree VectorBTree::zero(const VectorBTree& x) {
VectorBTree cloned(x);
return cloned.zero();
}
/* ************************************************************************* */
VectorBTree expmap(const VectorBTree& original, const VectorBTree& delta) {
check_compatible("VectorBTree:expmap", original, delta);
return original + delta;
}
/* ************************************************************************* */
VectorBTree expmap(const VectorBTree& original, const Vector& delta)
{
VectorBTree result = original;
result.values_ += delta;
return result;
}
/* ************************************************************************* */
void scal(double alpha, VectorBTree& x) {
scal(alpha, x.values_);
}
/* ************************************************************************* */
void axpy(double alpha, const VectorBTree& x, VectorBTree& y) {
check_cloned("VectorBTree:axpy", x, y);
axpy(alpha, x.values_, y.values_);
}
/* ************************************************************************* */
} // gtsam

258
cpp/VectorBTree.h Normal file
View File

@ -0,0 +1,258 @@
/**
* @file VectorBTree.h
* @brief Factor Graph Configuration
* @author Frank Dellaert
*/
// \callgraph
#pragma once
#include <map>
#include <boost/serialization/map.hpp>
#include <boost/numeric/ublas/storage.hpp>
#include "Testable.h"
#include "Vector.h"
#include "Key.h"
#include "BTree.h"
namespace gtsam {
/** Factor Graph Configuration */
class VectorBTree: public Testable<VectorBTree> {
private:
/** dictionary from Symbol to Range */
typedef boost::numeric::ublas::range Range;
typedef BTree<Symbol, Range> Ranges;
typedef Ranges::value_type Pair;
Ranges ranges_;
/** Actual vector */
Vector values_;
/** size_ is number of vectors */
size_t size_;
/** private get from symbol pair */
Vector get(const Range& r) const {
return sub(values_,r.start(),r.start()+r.size());
}
public:
/**
* Default constructor
*/
VectorBTree() :
size_(0) {
}
/**
* Copy constructor
*/
VectorBTree(const VectorBTree& c) :
ranges_(c.ranges_), values_(c.values_), size_(c.size_) {
}
/**
* Construct with a single vector
*/
VectorBTree(const Symbol& j, const Vector& a) :
size_(0) {
insert(j, a);
}
virtual ~VectorBTree() {
}
/** print */
void print(const std::string& name = "") const;
/** equals, for unit testing */
bool equals(const VectorBTree& expected, double tol = 1e-9) const;
/** Insert a value into the configuration with a given index: O(n) */
VectorBTree& insert(const Symbol& j, const Vector& v);
/** Insert or add a value with given index: O(n) if does not exist */
VectorBTree& insertAdd(const Symbol& j, const Vector& v);
/** Insert a config into another config, replace if key already exists */
void insert(const VectorBTree& config);
/** Insert a config into another config, add if key already exists */
void insertAdd(const VectorBTree& config);
/** Nr of vectors */
inline size_t size() const { return size_; }
/** Total dimensionality */
inline size_t dim() const { return values_.size(); }
/** Check whether Symbol j exists in config */
inline bool contains(const Symbol& j) const { return ranges_.mem(j); }
/** return all the nodes in the graph **/
std::vector<Symbol> get_names() const;
/** Vector access in VectorBtree is via the SubVector type */
SubVector operator[](const Symbol& j);
ConstSubVector operator[](const Symbol& j) const;
/** max of the vectors */
double max() const;
/**
* Check if compatible with other config, which is only
* guaranteed if vectors are inserted in exactly the same order,
* or if one config was created from the other using assignment.
* In the latter case, comparison is O(1), otherwise can be O(n).
*/
inline bool compatible(const VectorBTree& other) const {
return ranges_ == other.ranges_;
}
/**
* O(1) check if structure of config is *physically* the same.
* i.e., configs were created through some assignment chain.
*/
inline bool cloned(const VectorBTree& other) const {
return ranges_.same(other.ranges_);
}
/** Math operators */
VectorBTree scale(double s) const;
VectorBTree operator*(double s) const;
VectorBTree operator-() const;
void operator+=(const VectorBTree &b);
VectorBTree operator+(const VectorBTree &b) const;
void operator-=(const VectorBTree &b);
VectorBTree operator-(const VectorBTree &b) const;
double dot(const VectorBTree& b) const;
/** Set all vectors to zero */
VectorBTree& zero();
/** Create a clone of x with exactly same structure, except with zero values */
static VectorBTree zero(const VectorBTree& x);
/**
* Add a delta config, needed for use in NonlinearOptimizer
* For VectorBTree, this is just addition.
*/
friend VectorBTree expmap(const VectorBTree& original, const VectorBTree& delta);
/**
* Add a delta vector (not a config)
* Will use the ordering that map uses to loop over vectors
*/
friend VectorBTree expmap(const VectorBTree& original, const Vector& delta);
/**
* BLAS Level 1 scal: x <- alpha*x
*/
friend void scal(double alpha, VectorBTree& x);
/**
* BLAS Level 1 axpy: y <- alpha*x + y
* UNSAFE !!!! Only works if x and y laid out in exactly same shape
* Used in internal loop in iterative for fast conjugate gradients
* Consider using other functions if this is not in hotspot
*/
friend void axpy(double alpha, const VectorBTree& x, VectorBTree& y);
/** @brief Const iterator */
class const_iterator {
public:
// traits for playing nice with STL
typedef ptrdiff_t difference_type; // correct ?
typedef std::forward_iterator_tag iterator_category;
typedef std::pair<Symbol,Vector> value_type;
typedef const value_type* pointer;
typedef const value_type& reference;
bool operator==(const const_iterator& __x) const { return it_ == __x.it_;}
bool operator!=(const const_iterator& __x) const { return it_ != __x.it_;}
reference operator*() const { return value_;}
pointer operator->() const { return &value_;}
const_iterator& operator++() { increment(); return *this; }
const_iterator operator++(int) {
const_iterator __tmp = *this; increment(); return __tmp;
}
private:
Ranges::const_iterator it_, end_;
const VectorBTree& config_;
value_type value_;
const_iterator(const VectorBTree& config, const Ranges::const_iterator& it) :
it_(it), end_(config_.ranges_.end()), config_(config) {
update();
}
void update() {
if (it_ != end_) value_ = std::make_pair(it_->first, config_.get(it_->second));
}
void increment() { it_++; update();}
friend class VectorBTree;
}; // const_iterator
// We do not have a non-const iterator right now
typedef const_iterator iterator;
/** return iterators */
const_iterator begin() const { return const_iterator(*this,ranges_.begin());}
const_iterator end () const { return const_iterator(*this,ranges_.end());}
#ifdef UNTESTED
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(values);
}
}; // VectorBTree
#endif
}; // VectorBTree
/** scalar product */
inline VectorBTree operator*(double s, const VectorBTree& x) {
return x * s;
}
/** dim function (for iterative::CGD) */
inline double dim(const VectorBTree& x) {
return x.dim();
}
/** max of the vectors */
inline double max(const VectorBTree& x) {
return x.max();
}
/* dot product */
inline double dot(const VectorBTree& a, const VectorBTree& b) {
return a.dot(b);
}
/** print with optional string */
inline void print(const VectorBTree& v, const std::string& s = "") {
v.print(s);
}
} // gtsam

180
cpp/VectorConfig.h
View File

@ -1,165 +1,41 @@
/**
* @file VectorConfig.h
* @brief Factor Graph Configuration
* @author Carlos Nieto
* @author Christian Potthast
* @author Frank Dellaert
*/
// \callgraph
#pragma once
#include <map>
#include <boost/serialization/map.hpp>
/*
* There are two interchangeable implementations of VectorConfig.
*
* VectorMap uses a straightforward stl::map of Vectors. It has O(log n)
* insert and access, and is fairly fast at both. However, it has high overhead
* for arithmetic operations such as +, scale, axpy etc...
*
* VectorBTree uses a functional BTree as a way to access SubVectors
* in an ordinary Vector. Inserting is O(n) and much slower, but accessing,
* is O(log n) and might be a bit slower than VectorMap. Arithmetic operations
* are blindingly fast, however. The cost is it is not as KISS as VectorMap.
*
* Access to vectors is now exclusively via operator[]
* Vector access in VectorMap is via a Vector reference
* Vector access in VectorBtree is via the SubVector type (see Vector.h)
*
* Feb 16 2010: FD: I made VectorMap the default, because I decided to try
* and speed up conjugate gradients by using Sparse FactorGraphs all the way.
*/
#include "Testable.h"
#include "Vector.h"
#include "Key.h"
#include "SymbolMap.h"
// we use define and not typedefs as typdefs cannot be forward declared
#ifdef VECTORBTREE
namespace gtsam {
#include "VectorBTree.h"
#define VectorConfig VectorBTree
/** Factor Graph Configuration */
class VectorConfig : public Testable<VectorConfig> {
#else
protected:
/** Map from string indices to values */
SymbolMap<Vector> values;
#include "VectorMap.h"
#define VectorConfig VectorMap
public:
typedef SymbolMap<Vector>::iterator iterator;
typedef SymbolMap<Vector>::const_iterator const_iterator;
#endif
VectorConfig() {}
VectorConfig(const VectorConfig& cfg_in): values(cfg_in.values) {}
VectorConfig(const Symbol& j, const Vector& a) { add(j,a); }
virtual ~VectorConfig() {}
/** return all the nodes in the graph **/
std::vector<Symbol> get_names() const;
/** Insert a value into the configuration with a given index */
inline VectorConfig& insert(const Symbol& name, const Vector& val) {
values.insert(std::make_pair(name,val));
return *this;
}
/** Insert a config into another config */
void insert(const VectorConfig& config);
/** Add to vector at position j */
void add(const Symbol& j, const Vector& a);
const_iterator begin() const {return values.begin();}
const_iterator end() const {return values.end();}
iterator begin() {return values.begin();}
iterator end() {return values.end();}
/** get a vector in the configuration by name */
const Vector& get(const Symbol& name) const;
/** get a vector reference by name */
Vector& getReference(const Symbol& name);
/** operator[] syntax for get */
inline const Vector& operator[](const Symbol& name) const {
return get(name);
}
bool contains(const Symbol& name) const {
const_iterator it = values.find(name);
return (it!=values.end());
}
/** Nr of vectors */
size_t size() const { return values.size();}
/** Total dimensionality */
size_t dim() const;
/** max of the vectors */
inline double max() const {
double m = -std::numeric_limits<double>::infinity();
for(const_iterator it=begin(); it!=end(); it++)
m = std::max(m, gtsam::max(it->second));
return m;
}
/** Scale */
VectorConfig scale(double s) const;
VectorConfig operator*(double s) const;
/** Negation */
VectorConfig operator-() const;
/** Add in place */
void operator+=(const VectorConfig &b);
/** Add */
VectorConfig operator+(const VectorConfig &b) const;
/** Subtract */
VectorConfig operator-(const VectorConfig &b) const;
/** print */
void print(const std::string& name = "") const;
/** equals, for unit testing */
bool equals(const VectorConfig& expected, double tol=1e-9) const;
void clear() {values.clear();}
/** Dot product */
double dot(const VectorConfig& b) const;
/**
* Add a delta config, needed for use in NonlinearOptimizer
* For VectorConfig, this is just addition.
*/
friend VectorConfig expmap(const VectorConfig& original, const VectorConfig& delta);
/**
* Add a delta vector (not a config)
* Will use the ordering that map uses to loop over vectors
*/
friend VectorConfig expmap(const VectorConfig& original, const Vector& delta);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(values);
}
}; // VectorConfig
/** scalar product */
inline VectorConfig operator*(double s, const VectorConfig& x) {return x*s;}
/** Dot product */
double dot(const VectorConfig&, const VectorConfig&);
/**
* BLAS Level 1 scal: x <- alpha*x
*/
void scal(double alpha, VectorConfig& x);
/**
* BLAS Level 1 axpy: y <- alpha*x + y
* UNSAFE !!!! Only works if x and y laid out in exactly same shape
* Used in internal loop in iterative for fast conjugate gradients
* Consider using other functions if this is not in hotspot
*/
void axpy(double alpha, const VectorConfig& x, VectorConfig& y);
/** dim function (for iterative::CGD) */
inline double dim(const VectorConfig& x) { return x.dim();}
/** max of the vectors */
inline double max(const VectorConfig& x) { return x.max();}
/** print with optional string */
void print(const VectorConfig& v, const std::string& s = "");
} // gtsam

166
cpp/VectorConfig.cpp → cpp/VectorMap.cpp
View File

@ -1,14 +1,14 @@
/**
* @file VectorConfig.cpp
* @file VectorMap.cpp
* @brief Factor Graph Configuration
* @brief VectorConfig
* @brief VectorMap
* @author Carlos Nieto
* @author Christian Potthast
*/
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include "VectorConfig.h"
#include "VectorMap.h"
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
@ -23,12 +23,12 @@ void check_size(const Symbol& key, const Vector & vj, const Vector & dj) {
cout << "For key \"" << (string)key << "\"" << endl;
cout << "vj.size = " << vj.size() << endl;
cout << "dj.size = " << dj.size() << endl;
throw(std::invalid_argument("VectorConfig::+ mismatched dimensions"));
throw(std::invalid_argument("VectorMap::+ mismatched dimensions"));
}
}
/* ************************************************************************* */
std::vector<Symbol> VectorConfig::get_names() const {
std::vector<Symbol> VectorMap::get_names() const {
std::vector<Symbol> names;
for(const_iterator it=values.begin(); it!=values.end(); it++)
names.push_back(it->first);
@ -36,81 +36,106 @@ std::vector<Symbol> VectorConfig::get_names() const {
}
/* ************************************************************************* */
void VectorConfig::insert(const VectorConfig& config) {
for (const_iterator it = config.begin(); it!=config.end(); it++) {
insert(it->first, it->second);
}
VectorMap& VectorMap::insert(const Symbol& name, const Vector& v) {
values.insert(std::make_pair(name,v));
return *this;
}
/* ************************************************************************* */
void VectorConfig::add(const Symbol& j, const Vector& a) {
VectorMap& VectorMap::insertAdd(const Symbol& j, const Vector& a) {
Vector& vj = values[j];
if (vj.size()==0) vj = a; else vj += a;
return *this;
}
/* ************************************************************************* */
size_t VectorConfig::dim() const {
void VectorMap::insert(const VectorMap& config) {
for (const_iterator it = config.begin(); it!=config.end(); it++)
insert(it->first, it->second);
}
/* ************************************************************************* */
void VectorMap::insertAdd(const VectorMap& config) {
for (const_iterator it = config.begin(); it!=config.end(); it++)
insertAdd(it->first,it->second);
}
/* ************************************************************************* */
size_t VectorMap::dim() const {
size_t result=0;
Symbol key; Vector v; // rtodo: copying vector?
FOREACH_PAIR(key, v, values) result += v.size();
for (const_iterator it = begin(); it != end(); it++)
result += it->second.size();
return result;
}
/* ************************************************************************* */
VectorConfig VectorConfig::scale(double s) const {
VectorConfig scaled;
Symbol key; Vector val; // rtodo: copying vector?
FOREACH_PAIR(key, val, values)
scaled.insert(key, s*val);
const Vector& VectorMap::operator[](const Symbol& name) const {
return values.at(name);
}
/* ************************************************************************* */
Vector& VectorMap::operator[](const Symbol& name) {
return values.at(name);
}
/* ************************************************************************* */
VectorMap VectorMap::scale(double s) const {
VectorMap scaled;
for (const_iterator it = begin(); it != end(); it++)
scaled.insert(it->first, s*it->second);
return scaled;
}
/* ************************************************************************* */
VectorConfig VectorConfig::operator*(double s) const {
VectorMap VectorMap::operator*(double s) const {
return scale(s);
}
/* ************************************************************************* */
VectorConfig VectorConfig::operator-() const {
VectorConfig result;
Symbol j; Vector v; // rtodo: copying vector?
FOREACH_PAIR(j, v, values)
result.insert(j, -v);
VectorMap VectorMap::operator-() const {
VectorMap result;
for (const_iterator it = begin(); it != end(); it++)
result.insert(it->first, - it->second);
return result;
}
/* ************************************************************************* */
void VectorConfig::operator+=(const VectorConfig& b) {
Symbol j; Vector b_j; // rtodo: copying vector?
FOREACH_PAIR(j, b_j, b.values) {
iterator it = values.find(j);
if (it==values.end())
insert(j, b_j);
else
it->second += b_j;
}
void VectorMap::operator+=(const VectorMap& b) {
insertAdd(b);
}
/* ************************************************************************* */
VectorConfig VectorConfig::operator+(const VectorConfig& b) const {
VectorConfig result = *this;
VectorMap VectorMap::operator+(const VectorMap& b) const {
VectorMap result = *this;
result += b;
return result;
}
/* ************************************************************************* */
VectorConfig VectorConfig::operator-(const VectorConfig& b) const {
VectorConfig result;
Symbol j; Vector v; // rtodo: copying vector?
FOREACH_PAIR(j, v, values)
result.insert(j, v - b.get(j));
VectorMap VectorMap::operator-(const VectorMap& b) const {
VectorMap result;
for (const_iterator it = begin(); it != end(); it++)
result.insert(it->first, it->second - b[it->first]);
return result;
}
/* ************************************************************************* */
VectorConfig expmap(const VectorConfig& original, const VectorConfig& delta)
VectorMap& VectorMap::zero() {
for (iterator it = begin(); it != end(); it++)
std::fill(it->second.begin(), it->second.end(), 0.0);
return *this;
}
/* ************************************************************************* */
VectorMap VectorMap::zero(const VectorMap& x) {
VectorMap cloned(x);
return cloned.zero();
}
/* ************************************************************************* */
VectorMap expmap(const VectorMap& original, const VectorMap& delta)
{
VectorConfig newConfig;
VectorMap newConfig;
Symbol j; Vector vj; // rtodo: copying vector?
FOREACH_PAIR(j, vj, original.values) {
if (delta.contains(j)) {
@ -125,9 +150,9 @@ VectorConfig expmap(const VectorConfig& original, const VectorConfig& delta)
}
/* ************************************************************************* */
VectorConfig expmap(const VectorConfig& original, const Vector& delta)
VectorMap expmap(const VectorMap& original, const Vector& delta)
{
VectorConfig newConfig;
VectorMap newConfig;
size_t i = 0;
Symbol j; Vector vj; // rtodo: copying vector?
FOREACH_PAIR(j, vj, original.values) {
@ -140,70 +165,59 @@ VectorConfig expmap(const VectorConfig& original, const Vector& delta)
}
/* ************************************************************************* */
const Vector& VectorConfig::get(const Symbol& name) const {
return values.at(name);
}
/* ************************************************************************* */
Vector& VectorConfig::getReference(const Symbol& name) {
return values.at(name);
}
/* ************************************************************************* */
void VectorConfig::print(const string& name) const {
odprintf("VectorConfig %s\n", name.c_str());
void VectorMap::print(const string& name) const {
odprintf("VectorMap %s\n", name.c_str());
odprintf("size: %d\n", values.size());
Symbol j; Vector v; // rtodo: copying vector
FOREACH_PAIR(j, v, values) {
odprintf("%s:", ((string)j).c_str());
gtsam::print(v);
for (const_iterator it = begin(); it != end(); it++) {
odprintf("%s:", ((string)it->first).c_str());
gtsam::print(it->second);
}
}
/* ************************************************************************* */
bool VectorConfig::equals(const VectorConfig& expected, double tol) const {
Symbol j; Vector vActual; // rtodo: copying vector
bool VectorMap::equals(const VectorMap& expected, double tol) const {
if( values.size() != expected.size() ) return false;
// iterate over all nodes
FOREACH_PAIR(j, vActual, values) {
Vector vExpected = expected[j];
if(!equal_with_abs_tol(vExpected,vActual,tol))
for (const_iterator it = begin(); it != end(); it++) {
Vector vExpected = expected[it->first];
if(!equal_with_abs_tol(vExpected,it->second,tol))
return false;
}
return true;
}
/* ************************************************************************* */
double VectorConfig::dot(const VectorConfig& b) const {
Symbol j; Vector v; double result = 0.0; // rtodo: copying vector
FOREACH_PAIR(j, v, values) result += gtsam::dot(v,b.get(j));
double VectorMap::dot(const VectorMap& b) const {
double result = 0.0; // rtodo: copying vector
for (const_iterator it = begin(); it != end(); it++)
result += gtsam::dot(it->second,b[it->first]);
return result;
}
/* ************************************************************************* */
double dot(const VectorConfig& a, const VectorConfig& b) {
double dot(const VectorMap& a, const VectorMap& b) {
return a.dot(b);
}
/* ************************************************************************* */
void scal(double alpha, VectorConfig& x) {
for (VectorConfig::iterator xj = x.begin(); xj != x.end(); xj++)
void scal(double alpha, VectorMap& x) {
for (VectorMap::iterator xj = x.begin(); xj != x.end(); xj++)
scal(alpha, xj->second);
}
/* ************************************************************************* */
void axpy(double alpha, const VectorConfig& x, VectorConfig& y) {
VectorConfig::const_iterator xj = x.begin();
for (VectorConfig::iterator yj = y.begin(); yj != y.end(); yj++, xj++)
void axpy(double alpha, const VectorMap& x, VectorMap& y) {
VectorMap::const_iterator xj = x.begin();
for (VectorMap::iterator yj = y.begin(); yj != y.end(); yj++, xj++)
axpy(alpha, xj->second, yj->second);
}
/* ************************************************************************* */
void print(const VectorConfig& v, const std::string& s){
void print(const VectorMap& v, const std::string& s){
v.print(s);
}
/* ************************************************************************* */
} // gtsam

165
cpp/VectorMap.h Normal file
View File

@ -0,0 +1,165 @@
/**
* @file VectorMap.h
* @brief Factor Graph Configuration
* @author Carlos Nieto
* @author Christian Potthast
*/
// \callgraph
#pragma once
#include <map>
#include <boost/serialization/map.hpp>
#include "Testable.h"
#include "Vector.h"
#include "Key.h"
#include "SymbolMap.h"
namespace gtsam {
/** Factor Graph Configuration */
class VectorMap : public Testable<VectorMap> {
protected:
/** Map from string indices to values */
SymbolMap<Vector> values;
public:
typedef SymbolMap<Vector>::iterator iterator;
typedef SymbolMap<Vector>::const_iterator const_iterator;
VectorMap() {}
VectorMap(const VectorMap& cfg_in): values(cfg_in.values) {}
VectorMap(const Symbol& j, const Vector& a) { insert(j,a); }
virtual ~VectorMap() {}
/** return all the nodes in the graph **/
std::vector<Symbol> get_names() const;
/** Insert a value into the configuration with a given index */
VectorMap& insert(const Symbol& name, const Vector& v);
/** Insert or add a value with given index */
VectorMap& insertAdd(const Symbol& j, const Vector& v);
/** Insert a config into another config */
void insert(const VectorMap& config);
/** Insert a config into another config, add if key already exists */
void insertAdd(const VectorMap& config);
const_iterator begin() const {return values.begin();}
const_iterator end() const {return values.end();}
iterator begin() {return values.begin();}
iterator end() {return values.end();}
/** Vector access in VectorMap is via a Vector reference */
Vector& operator[](const Symbol& j);
const Vector& operator[](const Symbol& j) const;
bool contains(const Symbol& name) const {
const_iterator it = values.find(name);
return (it!=values.end());
}
/** Nr of vectors */
size_t size() const { return values.size();}
/** Total dimensionality */
size_t dim() const;
/** max of the vectors */
inline double max() const {
double m = -std::numeric_limits<double>::infinity();
for(const_iterator it=begin(); it!=end(); it++)
m = std::max(m, gtsam::max(it->second));
return m;
}
/** Scale */
VectorMap scale(double s) const;
VectorMap operator*(double s) const;
/** Negation */
VectorMap operator-() const;
/** Add in place */
void operator+=(const VectorMap &b);
/** Add */
VectorMap operator+(const VectorMap &b) const;
/** Subtract */
VectorMap operator-(const VectorMap &b) const;
/** print */
void print(const std::string& name = "") const;
/** equals, for unit testing */
bool equals(const VectorMap& expected, double tol=1e-9) const;
void clear() {values.clear();}
/** Dot product */
double dot(const VectorMap& b) const;
/** Set all vectors to zero */
VectorMap& zero();
/** Create a clone of x with exactly same structure, except with zero values */
static VectorMap zero(const VectorMap& x);
/**
* Add a delta config, needed for use in NonlinearOptimizer
* For VectorMap, this is just addition.
*/
friend VectorMap expmap(const VectorMap& original, const VectorMap& delta);
/**
* Add a delta vector (not a config)
* Will use the ordering that map uses to loop over vectors
*/
friend VectorMap expmap(const VectorMap& original, const Vector& delta);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(values);
}
}; // VectorMap
/** scalar product */
inline VectorMap operator*(double s, const VectorMap& x) {return x*s;}
/** Dot product */
double dot(const VectorMap&, const VectorMap&);
/**
* BLAS Level 1 scal: x <- alpha*x
*/
void scal(double alpha, VectorMap& x);
/**
* BLAS Level 1 axpy: y <- alpha*x + y
* UNSAFE !!!! Only works if x and y laid out in exactly same shape
* Used in internal loop in iterative for fast conjugate gradients
* Consider using other functions if this is not in hotspot
*/
void axpy(double alpha, const VectorMap& x, VectorMap& y);
/** dim function (for iterative::CGD) */
inline double dim(const VectorMap& x) { return x.dim();}
/** max of the vectors */
inline double max(const VectorMap& x) { return x.max();}
/** print with optional string */
void print(const VectorMap& v, const std::string& s = "");
} // gtsam

19
cpp/testBayesNetPreconditioner.cpp
View File

@ -89,9 +89,26 @@ TEST( BayesNetPreconditioner, conjugateGradients )
BOOST_FOREACH(const Symbol& j, ordering) y0.insert(j,z2);
VectorConfig y1 = y0;
y1.getReference("x2003") = Vector_(2, 1.0, -1.0);
y1["x2003"] = Vector_(2, 1.0, -1.0);
VectorConfig x1 = system.x(y1);
// Check gradient for y0
VectorConfig expectedGradient0;
expectedGradient0.insert("x1001", Vector_(2,-1000.,-1000.));
expectedGradient0.insert("x1002", Vector_(2, 0., -300.));
expectedGradient0.insert("x1003", Vector_(2, 0., -300.));
expectedGradient0.insert("x2001", Vector_(2, -100., 200.));
expectedGradient0.insert("x2002", Vector_(2, -100., 0.));
expectedGradient0.insert("x2003", Vector_(2, -100., -200.));
expectedGradient0.insert("x3001", Vector_(2, -100., 100.));
expectedGradient0.insert("x3002", Vector_(2, -100., 0.));
expectedGradient0.insert("x3003", Vector_(2, -100., -100.));
VectorConfig actualGradient0 = system.gradient(y0);
CHECK(assert_equal(expectedGradient0,actualGradient0));
#ifdef VECTORBTREE
CHECK(actualGradient0.cloned(y0));
#endif
// Solve using PCG
bool verbose = false;
double epsilon = 1e-6; // had to crank this down !!!

21
cpp/testGaussianFactor.cpp
View File

@ -670,26 +670,6 @@ TEST( GaussianFactor, CONSTRUCTOR_GaussianConditional )
CHECK(assert_equal(expectedLF,actualLF,1e-5));
}
/* ************************************************************************* */
TEST( GaussianFactor, alphaFactor )
{
GaussianFactorGraph fg = createGaussianFactorGraph();
// get alphafactor for first factor in fg at zero, in gradient direction
Symbol alphaKey(ALPHA, 1);
VectorConfig x = createZeroDelta();
VectorConfig d = fg.gradient(x);
GaussianFactor::shared_ptr factor = fg[0];
GaussianFactor::shared_ptr actual = factor->alphaFactor(alphaKey,x,d);
// calculate expected
Matrix A = Matrix_(2,1,300.0,50.0);
Vector b = Vector_(2,-1.0,-1.0);
GaussianFactor expected(alphaKey,A,b,noiseModel::Unit::Create(2));
CHECK(assert_equal(expected,*actual));
}
/* ************************************************************************* */
TEST ( GaussianFactor, constraint_eliminate1 )
{
@ -817,6 +797,7 @@ TEST ( GaussianFactor, combine_matrix ) {
// CHECK(true);
// }
//}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

2
cpp/testGaussianFactorGraph.cpp
View File

@ -31,7 +31,7 @@ using namespace example;
double tol=1e-5;
/* ************************************************************************* */
/* unit test for equals (GaussianFactorGraph1 == GaussianFactorGraph2) */
/* unit test for equals (GaussianFactorGraph1 == GaussianFactorGraph2) */
/* ************************************************************************* */
TEST( GaussianFactorGraph, equals ){

11
cpp/testIterative.cpp
View File

@ -14,7 +14,7 @@ using namespace boost::assign;
#define GTSAM_MAGIC_KEY
#include "Ordering.h"
#include "iterative.h"
#include "VectorConfig.h"
#include "smallExample.h"
#include "pose2SLAM.h"
#include "SubgraphPreconditioner.h"
@ -92,7 +92,7 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint )
Pose2Graph graph;
Matrix cov = eye(3);
graph.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(1.,0.,0.), cov)));
graph.addConstraint(1, config[1]);
graph.addHardConstraint(1, config[1]);
VectorConfig zeros;
zeros.insert("x1",zero(3));
@ -144,10 +144,6 @@ TEST( Iterative, subgraphPCG )
graph.addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
graph.addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1));
VectorConfig zeros;
zeros.insert("x1",zero(3));
zeros.insert("x2",zero(3));
// generate spanning tree and create ordering
PredecessorMap<Key> tree = graph.findMinimumSpanningTree<Key, Pose2Factor>();
list<Key> keys = predecessorMap2Keys(tree);
@ -168,6 +164,8 @@ TEST( Iterative, subgraphPCG )
SubgraphPreconditioner::sharedConfig xbar = optimize_(*Rc1);
SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar);
VectorConfig zeros = VectorConfig::zero(*xbar);
// Solve the subgraph PCG
VectorConfig ybar = conjugateGradients<SubgraphPreconditioner, VectorConfig,
Errors> (system, zeros, verbose, 1e-5, 1e-5, 100);
@ -178,6 +176,7 @@ TEST( Iterative, subgraphPCG )
expected.insert("x2", Vector_(3, -0.5, 0., 0.));
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
int main() {
TestResult tr;

33
cpp/testNonlinearOptimizer.cpp
View File

@ -205,25 +205,34 @@ TEST( NonlinearOptimizer, Factorization )
/* ************************************************************************* */
TEST( NonlinearOptimizer, SubgraphPCG )
{
typedef NonlinearOptimizer<Pose2Graph, Pose2Config, SubgraphPreconditioner, SubgraphPCG<Pose2Graph, Pose2Config> > Optimizer;
boost::shared_ptr<Pose2Config> config(new Pose2Config);
config->insert(1, Pose2(0.,0.,0.));
config->insert(2, Pose2(1.5,0.,0.));
typedef NonlinearOptimizer<Pose2Graph, Pose2Config, SubgraphPreconditioner,
SubgraphPCG<Pose2Graph, Pose2Config> > Optimizer;
// Create a graph
boost::shared_ptr<Pose2Graph> graph(new Pose2Graph);
graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1));
graph->addPrior(1, Pose2(0., 0., 0.), Isotropic::Sigma(3, 1e-10));
graph->addConstraint(1, 2, Pose2(1., 0., 0.), Isotropic::Sigma(3, 1));
// Create an initial config
boost::shared_ptr<Pose2Config> config(new Pose2Config);
config->insert(1, Pose2(0., 0., 0.));
config->insert(2, Pose2(1.5, 0., 0.));
// Create solver and optimizer
Optimizer::shared_solver solver
(new SubgraphPCG<Pose2Graph, Pose2Config> (*graph, *config));
Optimizer optimizer(graph, config, solver);
// Optimize !!!!
double relativeThreshold = 1e-5;
double absoluteThreshold = 1e-5;
Optimizer::shared_solver solver(new SubgraphPCG<Pose2Graph, Pose2Config>(*graph, *config));
Optimizer optimizer(graph, config, solver);
Optimizer optimized = optimizer.gaussNewton(relativeThreshold, absoluteThreshold, Optimizer::SILENT);
Optimizer optimized = optimizer.gaussNewton(relativeThreshold,
absoluteThreshold, Optimizer::SILENT);
// Check solution
Pose2Config expected;
expected.insert(1, Pose2(0.,0.,0.));
expected.insert(2, Pose2(1.,0.,0.));
expected.insert(1, Pose2(0., 0., 0.));
expected.insert(2, Pose2(1., 0., 0.));
CHECK(assert_equal(expected, *optimized.config(), 1e-5));
}

3
cpp/testPose2Factor.cpp
View File

@ -50,7 +50,8 @@ TEST( Pose2Factor, error )
CHECK(assert_equal(-error_at_zero,linear->error_vector(delta)));
// Check error after increasing p2
VectorConfig plus = delta + VectorConfig("x2", Vector_(3, 0.1, 0.0, 0.0));
VectorConfig plus = delta;
plus.insertAdd("x2", Vector_(3, 0.1, 0.0, 0.0));
Pose2Config x1 = expmap(x0, plus);
Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
CHECK(assert_equal(error_at_plus,factor.whitenedError(x1)));

69
cpp/testSubgraphPreconditioner.cpp
View File

@ -66,29 +66,6 @@ TEST( SubgraphPreconditioner, splitOffPlanarTree )
CHECK(assert_equal(xtrue,xbar));
}
/* ************************************************************************* */
double error(const VectorConfig& x) {
// Build a planar graph
GaussianFactorGraph Ab;
VectorConfig xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
// Eliminate the spanning tree to build a prior
Ordering ordering = planarOrdering(N);
SubgraphPreconditioner::sharedBayesNet Rc1 = Ab1_.eliminate_(ordering); // R1*x-c1
SubgraphPreconditioner::sharedConfig xbar = optimize_(*Rc1); // xbar = inv(R1)*c1
SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar);
return system.error(x);
}
/* ************************************************************************* */
TEST( SubgraphPreconditioner, system )
{
@ -113,21 +90,19 @@ TEST( SubgraphPreconditioner, system )
SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar);
// Create zero config
VectorConfig zeros;
Vector z2 = zero(2);
BOOST_FOREACH(const Symbol& j, ordering) zeros.insert(j,z2);
VectorConfig zeros = VectorConfig::zero(*xbar);
// Set up y0 as all zeros
VectorConfig y0 = zeros;
// y1 = perturbed y0
VectorConfig y1 = zeros;
y1.getReference("x2003") = Vector_(2, 1.0, -1.0);
y1["x2003"] = Vector_(2, 1.0, -1.0);
// Check corresponding x values
VectorConfig expected_x1 = xtrue, x1 = system.x(y1);
expected_x1.getReference("x2003") = Vector_(2, 2.01, 2.99);
expected_x1.getReference("x3003") = Vector_(2, 3.01, 2.99);
expected_x1["x2003"] = Vector_(2, 2.01, 2.99);
expected_x1["x3003"] = Vector_(2, 3.01, 2.99);
CHECK(assert_equal(xtrue, system.x(y0)));
CHECK(assert_equal(expected_x1,system.x(y1)));
@ -141,30 +116,30 @@ TEST( SubgraphPreconditioner, system )
VectorConfig expected_gx0 = zeros;
VectorConfig expected_gx1 = zeros;
CHECK(assert_equal(expected_gx0,Ab.gradient(xtrue)));
expected_gx1.getReference("x1003") = Vector_(2, -100., 100.);
expected_gx1.getReference("x2002") = Vector_(2, -100., 100.);
expected_gx1.getReference("x2003") = Vector_(2, 200., -200.);
expected_gx1.getReference("x3002") = Vector_(2, -100., 100.);
expected_gx1.getReference("x3003") = Vector_(2, 100., -100.);
expected_gx1["x1003"] = Vector_(2, -100., 100.);
expected_gx1["x2002"] = Vector_(2, -100., 100.);
expected_gx1["x2003"] = Vector_(2, 200., -200.);
expected_gx1["x3002"] = Vector_(2, -100., 100.);
expected_gx1["x3003"] = Vector_(2, 100., -100.);
CHECK(assert_equal(expected_gx1,Ab.gradient(x1)));
// Test gradient in y
VectorConfig expected_gy0 = zeros;
VectorConfig expected_gy1 = zeros;
expected_gy1.getReference("x1003") = Vector_(2, 2., -2.);
expected_gy1.getReference("x2002") = Vector_(2, -2., 2.);
expected_gy1.getReference("x2003") = Vector_(2, 3., -3.);
expected_gy1.getReference("x3002") = Vector_(2, -1., 1.);
expected_gy1.getReference("x3003") = Vector_(2, 1., -1.);
expected_gy1["x1003"] = Vector_(2, 2., -2.);
expected_gy1["x2002"] = Vector_(2, -2., 2.);
expected_gy1["x2003"] = Vector_(2, 3., -3.);
expected_gy1["x3002"] = Vector_(2, -1., 1.);
expected_gy1["x3003"] = Vector_(2, 1., -1.);
CHECK(assert_equal(expected_gy0,system.gradient(y0)));
CHECK(assert_equal(expected_gy1,system.gradient(y1)));
// Check it numerically for good measure
// TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1)
Vector numerical_g1 = numericalGradient<VectorConfig> (error, y1, 0.001);
Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2.,
3., -3., 0., 0., -1., 1., 1., -1.);
CHECK(assert_equal(expected_g1,numerical_g1));
// Vector numerical_g1 = numericalGradient<VectorConfig> (error, y1, 0.001);
// Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2.,
// 3., -3., 0., 0., -1., 1., 1., -1.);
// CHECK(assert_equal(expected_g1,numerical_g1));
}
/* ************************************************************************* */
@ -191,12 +166,10 @@ TEST( SubgraphPreconditioner, conjugateGradients )
SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar);
// Create zero config y0 and perturbed config y1
VectorConfig y0;
Vector z2 = zero(2);
BOOST_FOREACH(const string& j, ordering) y0.insert(j,z2);
VectorConfig y0 = VectorConfig::zero(*xbar);
VectorConfig y1 = y0;
y1.getReference("x2003") = Vector_(2, 1.0, -1.0);
y1["x2003"] = Vector_(2, 1.0, -1.0);
VectorConfig x1 = system.x(y1);
// Solve for the remaining constraints using PCG
@ -209,7 +182,7 @@ TEST( SubgraphPreconditioner, conjugateGradients )
// Compare with non preconditioned version:
VectorConfig actual2 = conjugateGradientDescent(Ab, x1, verbose, epsilon,
maxIterations);
epsilon, maxIterations);
CHECK(assert_equal(xtrue,actual2,1e-4));
}

310
cpp/testVectorBTree.cpp Normal file
View File

@ -0,0 +1,310 @@
/**
* @file testVectorBTree.cpp
* @brief Unit tests for Factor Graph Configuration
* @author Frank Dellaert
**/
#include <iostream>
#include <sstream>
#include <limits>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign; // bring 'operator+=()' into scope
//#include TEST_AC_DEFINE
#ifdef HAVE_BOOST_SERIALIZATION
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#endif //HAVE_BOOST_SERIALIZATION
#include <CppUnitLite/TestHarness.h>
#include "Matrix.h"
#include "VectorBTree.h"
using namespace std;
using namespace gtsam;
static Symbol l1('l',1), x1('x',1), x2('x',2);
static double inf = std::numeric_limits<double>::infinity();
/* ************************************************************************* */
VectorBTree smallVectorBTree() {
VectorBTree c;
c.insert(l1, Vector_(2, 0.0, -1.0));
c.insert(x1, Vector_(2, 0.0, 0.0));
c.insert(x2, Vector_(2, 1.5, 0.0));
return c;
}
/* ************************************************************************* */
TEST( VectorBTree, constructor_insert_get )
{
VectorBTree expected;
Vector v = Vector_(3, 5.0, 6.0, 7.0);
expected.insert(x1, v);
VectorBTree actual(x1, v);
LONGS_EQUAL(1,actual.size())
CHECK(assert_equal(expected,actual))
CHECK(equal_with_abs_tol(v,actual[x1]))
}
/* ************************************************************************* */
TEST( VectorBTree, dim) {
VectorBTree c = smallVectorBTree();
LONGS_EQUAL(6,c.dim());
}
/* ************************************************************************* */
TEST( VectorBTree, insertAdd) {
VectorBTree expected;
expected.insert(l1, Vector_(2, 0.0, -2.0));
expected.insert(x1, Vector_(2, 0.0, 0.0));
expected.insert(x2, Vector_(2, 3.0, 0.0));
VectorBTree actual = smallVectorBTree();
actual.insertAdd(actual);
CHECK(assert_equal(expected,actual))
}
/* ************************************************************************* */
TEST( VectorBTree, zero) {
VectorBTree expected;
expected.insert(l1, Vector_(2, 0.0, 0.0));
expected.insert(x1, Vector_(2, 0.0, 0.0));
expected.insert(x2, Vector_(2, 0.0, 0.0));
VectorBTree actual = smallVectorBTree();
CHECK(assert_equal(expected,VectorBTree::zero(actual)));
CHECK(assert_equal(expected,actual.zero()));
}
/* ************************************************************************* */
TEST( VectorBTree, insert_config) {
VectorBTree expected = smallVectorBTree();
VectorBTree actual, toAdd = smallVectorBTree();
actual.insert(toAdd);
CHECK(assert_equal(expected,actual))
}
/* ************************************************************************* */
TEST( VectorBTree, get_names) {
VectorBTree c = smallVectorBTree();
std::vector<Symbol> expected, actual = c.get_names();
expected += l1, x1, x2;
CHECK(expected==actual)
}
/* ************************************************************************* */
TEST( VectorBTree, const_iterator) {
VectorBTree c = smallVectorBTree();
VectorBTree::const_iterator it = c.begin();
CHECK(assert_equal(l1,it->first));
CHECK(assert_equal(Vector_(2, 0.0,-1.0),(it++)->second));
CHECK(assert_equal(x1,it->first));
CHECK(assert_equal(Vector_(2, 0.0, 0.0),(it++)->second));
CHECK(assert_equal(x2,it->first));
CHECK(assert_equal(Vector_(2, 1.5, 0.0),(it++)->second));
CHECK(it==c.end());
}
/* ************************************************************************* */
TEST( VectorBTree, equals )
{
VectorBTree cfg1;
cfg1.insert(x1, Vector_(3, 5.0, 6.0, 7.0));
CHECK(cfg1.equals(cfg1));
CHECK(cfg1.compatible(cfg1));
CHECK(cfg1.cloned(cfg1));
VectorBTree cfg2;
cfg2.insert(x1, Vector_(3, 5.0, 6.0, 7.0));
CHECK(cfg1.equals(cfg2));
CHECK(cfg1.compatible(cfg2));
CHECK(!cfg1.cloned(cfg2));
VectorBTree cfg3 = cfg1;
CHECK(cfg1.equals(cfg3));
CHECK(cfg1.compatible(cfg3));
CHECK(cfg1.cloned(cfg3));
VectorBTree cfg4;
cfg4.insert(x1, Vector_(3, 5.0, 6.0, 8.0));
CHECK(!cfg1.equals(cfg4));
CHECK(cfg1.compatible(cfg4));
CHECK(!cfg1.cloned(cfg4));
}
/* ************************************************************************* */
TEST( VectorBTree, equals_nan )
{
VectorBTree cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, inf, inf, inf);
cfg1.insert(x1, v1);
cfg2.insert(x1, v2);
CHECK(!cfg1.equals(cfg2));
CHECK(!cfg2.equals(cfg1));
}
/* ************************************************************************* */
TEST( VectorBTree, contains)
{
VectorBTree fg;
Vector v = Vector_(3, 5.0, 6.0, 7.0);
fg.insert(x1, v);
CHECK(fg.contains(x1));
CHECK(!fg.contains(x2));
}
/* ************************************************************************* */
TEST( VectorBTree, max) {
VectorBTree c = smallVectorBTree();
DOUBLES_EQUAL(1.5,c.max(),1e-9);
}
/* ************************************************************************* */
TEST( VectorBTree, scale) {
VectorBTree cfg;
cfg.insert(x1, Vector_(2, 1.0, 2.0));
cfg.insert(x2, Vector_(2,-1.0,-2.0));
VectorBTree actual = cfg.scale(2.0);
VectorBTree expected;
expected.insert(x1, Vector_(2, 2.0, 4.0));
expected.insert(x2, Vector_(2,-2.0,-4.0));
CHECK(assert_equal(actual, expected));
}
/* ************************************************************************* */
TEST( VectorBTree, plus)
{
VectorBTree c;
Vector vx = Vector_(3, 5.0, 6.0, 7.0), vy = Vector_(2, 8.0, 9.0);
c.insert(x1,vx).insert(x2,vy);
VectorBTree delta;
Vector dx = Vector_(3, 1.0, 1.0, 1.0), dy = Vector_(2, -1.0, -1.0);
delta.insert(x1, dx).insert(x2,dy);
CHECK(delta.compatible(c));
// operator +
VectorBTree expected;
Vector wx = Vector_(3, 6.0, 7.0, 8.0), wy = Vector_(2, 7.0, 8.0);
expected.insert(x1, wx).insert(x2,wy);
CHECK(assert_equal(expected,c+delta));
// operator -
VectorBTree expected2;
Vector wx2 = Vector_(3, -5.0, -6.0, -7.0), wy2 = Vector_(2, -8.0, -9.0);
expected2.insert(x1, wx2).insert(x2,wy2);
CHECK(assert_equal(expected2,-c));
// expmap
VectorBTree actual = expmap(c,delta);
CHECK(assert_equal(expected,actual));
// in-place (although + already tests that, really)
c += delta;
CHECK(assert_equal(expected,c));
}
/* ************************************************************************* */
TEST( VectorBTree, operators) {
VectorBTree c; c.insert(x1, Vector_(2, 1.1, 2.2));
VectorBTree expected1; expected1.insert(x1, Vector_(2, 2.2, 4.4));
CHECK(assert_equal(expected1,c*2));
CHECK(assert_equal(expected1,c+c));
VectorBTree expected2; expected2.insert(x1, Vector_(2, 0.0, 0.0));
CHECK(assert_equal(expected2,c-c));
}
/* ************************************************************************* */
TEST( VectorBTree, dot) {
VectorBTree c = smallVectorBTree();
DOUBLES_EQUAL(3.25,dot(c,c),1e-9);
}
/* ************************************************************************* */
TEST( VectorBTree, expmap)
{
VectorBTree c = smallVectorBTree();
Vector v = Vector_(6, 0.0,-1.0, 0.0, 0.0, 1.5, 0.0); // l1, x1, x2
CHECK(assert_equal(expmap(c,c),expmap(c,v)));
}
/* ************************************************************************* */
TEST( VectorBTree, scal) {
VectorBTree x,expected;
x.insert(x1,Vector_(3, 1.0, 2.0, 3.0));
x.insert(x2,Vector_(2, 4.0, 5.0));
expected.insert(x1,Vector_(3, 10.0, 20.0, 30.0));
expected.insert(x2,Vector_(2, 40.0, 50.0));
scal(10,x);
CHECK(assert_equal(expected,x));
}
/* ************************************************************************* */
TEST( VectorBTree, axpy) {
VectorBTree x;
x.insert(x1,Vector_(3, 1.0, 1.0, 1.0));
x.insert(x2,Vector_(2, -1.0, -1.0));
// axpy will only work on cloned configs - enforced for speed
VectorBTree y = VectorBTree::zero(x);
y[x1] = Vector_(3, 5.0, 6.0, 7.0);
y[x2] = Vector_(2, 8.0, 9.0);
// axpy(10,x,y);
//
// // Check result
// VectorBTree expected;
// expected.insert(x1,Vector_(3, 15.0, 16.0, 17.0));
// expected.insert(x2,Vector_(2, -2.0, -1.0));
// CHECK(assert_equal(expected,y));
}
/* ************************************************************************* */
TEST( VectorBTree, subVector) {
VectorBTree c; c.insert(x1, Vector_(2, 1.1, 2.2));
SubVector cx = c[x1];
for (size_t i = 0; i < 2; i++)
cx(i) = cx(i)*2.0;
VectorBTree expected; expected.insert(x1, Vector_(2, 2.2, 4.4));
CHECK(assert_equal(expected,c));
}
/* ************************************************************************* */
#ifdef HAVE_BOOST_SERIALIZATION
TEST( VectorBTree, serialize)
{
//DEBUG:
cout << "VectorBTree: Running Serialization Test" << endl;
//create an VectorBTree
VectorBTree fg = createConfig();
//serialize the config
std::ostringstream in_archive_stream;
boost::archive::text_oarchive in_archive(in_archive_stream);
in_archive << fg;
std::string serialized_fgc = in_archive_stream.str();
//deserialize the config
std::istringstream out_archive_stream(serialized_fgc);
boost::archive::text_iarchive out_archive(out_archive_stream);
VectorBTree output;
out_archive >> output;
//check for equality
CHECK(fg.equals(output));
}
#endif //HAVE_BOOST_SERIALIZATION
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

131
cpp/testVectorConfig.cpp → cpp/testVectorMap.cpp
View File

@ -1,5 +1,5 @@
/**
* @file testVectorConfig.cpp
* @file testVectorMap.cpp
* @brief Unit tests for Factor Graph Configuration
* @author Carlos Nieto
**/
@ -19,28 +19,37 @@
#include <CppUnitLite/TestHarness.h>
#include "Matrix.h"
#include "VectorConfig.h"
#include "smallExample.cpp"
#include "VectorMap.h"
using namespace std;
using namespace gtsam;
using namespace example;
static Symbol l1('l',1), x1('x',1), x2('x',2);
/* ************************************************************************* */
TEST( VectorConfig, equals1 )
VectorMap smallVectorMap() {
VectorMap c;
c.insert(l1, Vector_(2, 0.0, -1.0));
c.insert(x1, Vector_(2, 0.0, 0.0));
c.insert(x2, Vector_(2, 1.5, 0.0));
return c;
}
/* ************************************************************************* */
TEST( VectorMap, equals1 )
{
VectorConfig expected;
VectorMap expected;
Vector v = Vector_(3, 5.0, 6.0, 7.0);
expected.insert("a",v);
VectorConfig actual;
VectorMap actual;
actual.insert("a",v);
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( VectorConfig, equals2 )
TEST( VectorMap, equals2 )
{
VectorConfig cfg1, cfg2;
VectorMap cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 5.0, 6.0, 8.0);
cfg1.insert("x", v1);
@ -54,9 +63,9 @@ TEST( VectorConfig, equals2 )
#include <limits>
double inf = std::numeric_limits<double>::infinity();
TEST( VectorConfig, equals_nan )
TEST( VectorMap, equals_nan )
{
VectorConfig cfg1, cfg2;
VectorMap cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, inf, inf, inf);
cfg1.insert("x", v1);
@ -66,9 +75,9 @@ TEST( VectorConfig, equals_nan )
}
/* ************************************************************************* */
TEST( VectorConfig, contains)
TEST( VectorMap, contains)
{
VectorConfig fg;
VectorMap fg;
Vector v = Vector_(3, 5.0, 6.0, 7.0);
fg.insert("a", v);
CHECK(fg.contains("a"));
@ -76,43 +85,43 @@ TEST( VectorConfig, contains)
}
/* ************************************************************************* */
TEST( VectorConfig, expmap)
TEST( VectorMap, expmap)
{
VectorConfig c = createVectorConfig();
VectorMap c = smallVectorMap();
Vector v = Vector_(6, 0.0,-1.0, 0.0, 0.0, 1.5, 0.0); // l1, x1, x2
CHECK(assert_equal(expmap(c,c),expmap(c,v)));
}
/* ************************************************************************* */
TEST( VectorConfig, plus)
TEST( VectorMap, plus)
{
VectorConfig c;
VectorMap c;
Vector vx = Vector_(3, 5.0, 6.0, 7.0), vy = Vector_(2, 8.0, 9.0);
c += VectorConfig("x",vx);
c += VectorConfig("y",vy);
c += VectorMap("x",vx);
c += VectorMap("y",vy);
VectorConfig delta;
VectorMap delta;
Vector dx = Vector_(3, 1.0, 1.0, 1.0), dy = Vector_(2, -1.0, -1.0);
delta.insert("x", dx).insert("y",dy);
VectorConfig expected;
VectorMap expected;
Vector wx = Vector_(3, 6.0, 7.0, 8.0), wy = Vector_(2, 7.0, 8.0);
expected.insert("x", wx).insert("y",wy);
// functional
VectorConfig actual = expmap(c,delta);
VectorMap actual = expmap(c,delta);
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( VectorConfig, scale) {
VectorConfig cfg;
TEST( VectorMap, scale) {
VectorMap cfg;
cfg.insert("x", Vector_(2, 1.0, 2.0));
cfg.insert("y", Vector_(2,-1.0,-2.0));
VectorConfig actual = cfg.scale(2.0);
VectorMap actual = cfg.scale(2.0);
VectorConfig expected;
VectorMap expected;
expected.insert("x", Vector_(2, 2.0, 4.0));
expected.insert("y", Vector_(2,-2.0,-4.0));
@ -120,42 +129,42 @@ TEST( VectorConfig, scale) {
}
/* ************************************************************************* */
TEST( VectorConfig, axpy) {
VectorConfig x,y,expected;
x += VectorConfig("x",Vector_(3, 1.0, 1.0, 1.0));
x += VectorConfig("y",Vector_(2, -1.0, -1.0));
y += VectorConfig("x",Vector_(3, 5.0, 6.0, 7.0));
y += VectorConfig("y",Vector_(2, 8.0, 9.0));
expected += VectorConfig("x",Vector_(3, 15.0, 16.0, 17.0));
expected += VectorConfig("y",Vector_(2, -2.0, -1.0));
TEST( VectorMap, axpy) {
VectorMap x,y,expected;
x += VectorMap("x",Vector_(3, 1.0, 1.0, 1.0));
x += VectorMap("y",Vector_(2, -1.0, -1.0));
y += VectorMap("x",Vector_(3, 5.0, 6.0, 7.0));
y += VectorMap("y",Vector_(2, 8.0, 9.0));
expected += VectorMap("x",Vector_(3, 15.0, 16.0, 17.0));
expected += VectorMap("y",Vector_(2, -2.0, -1.0));
axpy(10,x,y);
CHECK(assert_equal(expected,y));
}
/* ************************************************************************* */
TEST( VectorConfig, scal) {
VectorConfig x,expected;
x += VectorConfig("x",Vector_(3, 1.0, 2.0, 3.0));
x += VectorConfig("y",Vector_(2, 4.0, 5.0));
expected += VectorConfig("x",Vector_(3, 10.0, 20.0, 30.0));
expected += VectorConfig("y",Vector_(2, 40.0, 50.0));
TEST( VectorMap, scal) {
VectorMap x,expected;
x += VectorMap("x",Vector_(3, 1.0, 2.0, 3.0));
x += VectorMap("y",Vector_(2, 4.0, 5.0));
expected += VectorMap("x",Vector_(3, 10.0, 20.0, 30.0));
expected += VectorMap("y",Vector_(2, 40.0, 50.0));
scal(10,x);
CHECK(assert_equal(expected,x));
}
/* ************************************************************************* */
TEST( VectorConfig, update_with_large_delta) {
TEST( VectorMap, update_with_large_delta) {
// this test ensures that if the update for delta is larger than
// the size of the config, it only updates existing variables
VectorConfig init, delta;
VectorMap init, delta;
init.insert("x", Vector_(2, 1.0, 2.0));
init.insert("y", Vector_(2, 3.0, 4.0));
delta.insert("x", Vector_(2, 0.1, 0.1));
delta.insert("y", Vector_(2, 0.1, 0.1));
delta.insert("p", Vector_(2, 0.1, 0.1));
VectorConfig actual = expmap(init,delta);
VectorConfig expected;
VectorMap actual = expmap(init,delta);
VectorMap expected;
expected.insert("x", Vector_(2, 1.1, 2.1));
expected.insert("y", Vector_(2, 3.1, 4.1));
@ -163,45 +172,45 @@ TEST( VectorConfig, update_with_large_delta) {
}
/* ************************************************************************* */
TEST( VectorConfig, dot) {
VectorConfig c = createVectorConfig();
TEST( VectorMap, dot) {
VectorMap c = smallVectorMap();
DOUBLES_EQUAL(3.25,dot(c,c),1e-9);
}
/* ************************************************************************* */
TEST( VectorConfig, dim) {
VectorConfig c = createVectorConfig();
TEST( VectorMap, dim) {
VectorMap c = smallVectorMap();
LONGS_EQUAL(6,c.dim());
}
/* ************************************************************************* */
TEST( VectorConfig, operators) {
VectorConfig c; c.insert("x", Vector_(2, 1.1, 2.2));
VectorConfig expected1; expected1.insert("x", Vector_(2, 2.2, 4.4));
TEST( VectorMap, operators) {
VectorMap c; c.insert("x", Vector_(2, 1.1, 2.2));
VectorMap expected1; expected1.insert("x", Vector_(2, 2.2, 4.4));
CHECK(assert_equal(expected1,c*2));
CHECK(assert_equal(expected1,c+c));
VectorConfig expected2; expected2.insert("x", Vector_(2, 0.0, 0.0));
VectorMap expected2; expected2.insert("x", Vector_(2, 0.0, 0.0));
CHECK(assert_equal(expected2,c-c));
}
/* ************************************************************************* */
TEST( VectorConfig, getReference) {
VectorConfig c; c.insert("x", Vector_(2, 1.1, 2.2));
Vector& cx = c.getReference("x");
TEST( VectorMap, getReference) {
VectorMap c; c.insert("x", Vector_(2, 1.1, 2.2));
Vector& cx = c["x"];
cx = cx*2.0;
VectorConfig expected; expected.insert("x", Vector_(2, 2.2, 4.4));
VectorMap expected; expected.insert("x", Vector_(2, 2.2, 4.4));
CHECK(assert_equal(expected,c));
}
/* ************************************************************************* */
#ifdef HAVE_BOOST_SERIALIZATION
TEST( VectorConfig, serialize)
TEST( VectorMap, serialize)
{
//DEBUG:
cout << "VectorConfig: Running Serialization Test" << endl;
cout << "VectorMap: Running Serialization Test" << endl;
//create an VectorConfig
VectorConfig fg = createConfig();
//create an VectorMap
VectorMap fg = createConfig();
//serialize the config
std::ostringstream in_archive_stream;
@ -212,7 +221,7 @@ TEST( VectorConfig, serialize)
//deserialize the config
std::istringstream out_archive_stream(serialized_fgc);
boost::archive::text_iarchive out_archive(out_archive_stream);
VectorConfig output;
VectorMap output;
out_archive >> output;
//check for equality

76
cpp/timeVectorConfig.cpp
View File

@ -6,29 +6,18 @@
#include <iostream>
#include <boost/timer.hpp>
#include "VectorConfig.h"
#include "VectorBTree.h"
#include "VectorMap.h"
using namespace std;
using namespace gtsam;
#define TIME(STATEMENT) { boost::timer t; \
for (int j = 0; j < n; ++j) STATEMENT; \
#define TIME1(STATEMENT) { boost::timer t; \
STATEMENT; \
double time = t.elapsed(); \
cout << "Average elapsed time :" << 10e3 * time / n << "ms." << endl; }
/* ************************************************************************* */
void unsafe_assign(VectorConfig& a, const VectorConfig& b) {
VectorConfig::const_iterator bp = b.begin();
for (VectorConfig::iterator ap = a.begin(); ap != a.end(); ap++, bp++)
ap->second = bp->second;
}
/* ************************************************************************* */
void unsafe_add(VectorConfig& a, const VectorConfig& b) {
VectorConfig::const_iterator bp = b.begin();
for (VectorConfig::iterator ap = a.begin(); ap != a.end(); ap++, bp++)
ap->second += bp->second;
}
#define TIME(STATEMENT) TIME1(for (int j = 0; j < n; ++j) STATEMENT;)
/* ************************************************************************* */
int main(int argc, char ** argv) {
@ -39,35 +28,42 @@ int main(int argc, char ** argv) {
// n = number of times to loop
// m = number of vectors
// r = rows per vector
size_t n = 100, m = 10000, r = 3, alpha = 0.1;
size_t n = 100, m = 30000, r = 3, alpha = 0.1;
// Create 2 VectorConfigs
VectorConfig x, y;
for (int i = 0; i < m; ++i) {
Vector v = zero(r);
Symbol key('x', i);
x.add(key, v);
y.add(key, v);
{
cout << "Vector:" << endl;
Vector v = zero(m * r), w = zero(m * r);
TIME(v=w);
TIME(v+=w);
TIME(v+=alpha*w);
TIME(axpy(alpha,v,w));
}
cout << "Convenient VectorConfig:" << endl;
TIME(x=y);
TIME(x+=y);
TIME(x+=alpha*y);
//TIME(a=a+b);
{
// Create 2 VectorBTrees and one VectorMap
VectorBTree p, q;
VectorMap old;
cout << "Creating VectorBTree:" << endl;
TIME1(for (int i = 0; i < m; ++i) {
Vector v = zero(r);
Symbol key('x', i);
p.insert(key, v);
q.insert(key, v);
old.insert(key, v);
})
cout << "Unsafe VectorConfig:" << endl;
TIME(unsafe_assign(x,y));
TIME(unsafe_add(x,y));
TIME(axpy(alpha,x,y));
cout << "VectorBTree:" << endl;
TIME(p=q);
TIME(p+=q);
TIME(p+=alpha*q);
TIME(axpy(alpha,p,q));
cout << "Compares with Vector:" << endl;
Vector v = zero(m * r), w = zero(m * r);
TIME(v=w);
TIME(v+=w);
TIME(v+=alpha*w);
TIME(axpy(alpha,v,w));
// TIME(v=v+w);
cout << "VectorBTree get:" << endl;
TIME1(for (int i = 0; i < m; ++i) p[Symbol('x', i)]);
cout << "VectorMap get:" << endl;
TIME1(for (int i = 0; i < m; ++i) old[Symbol('x', i)]);
}
return 0;
}