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

pose2slamoptimizer unit tests worked

release/4.3a0
Kai Ni 2010-01-23 03:49:05 +00:00
parent 21c1af2b9f
commit b092fee64b
9 changed files with 672 additions and 597 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="false" 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="true" 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,7 +469,6 @@
</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>
@ -477,6 +476,7 @@
</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,7 +484,6 @@
</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>
@ -708,7 +707,6 @@
</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>
@ -764,6 +762,7 @@
</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>
@ -785,6 +784,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2SLAM" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testPose2SLAM</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</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></value>
<value>-j2</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildCommand</key>

22
cpp/GaussianFactorGraph.cpp
View File

@ -198,8 +198,8 @@ Errors GaussianFactorGraph::rhs() const {
/* ************************************************************************* */
Vector GaussianFactorGraph::rhsVector() const {
Vector v;
return v;
Errors e = rhs();
return concatVectors(e);
}
/* ************************************************************************* */
@ -217,6 +217,24 @@ pair<Matrix,Vector> GaussianFactorGraph::matrix(const Ordering& ordering) const
return lf.matrix(ordering);
}
/* ************************************************************************* */
VectorConfig GaussianFactorGraph::assembleConfig(const Vector& vs, const Ordering& ordering) const {
Dimensions dims = dimensions();
VectorConfig config;
Vector::const_iterator itSrc = vs.begin();
Vector::iterator itDst;
BOOST_FOREACH(const Symbol& key, ordering){
int dim = dims.find(key)->second;
Vector v(dim);
for (itDst=v.begin(); itDst!=v.end(); itDst++, itSrc++)
*itDst = *itSrc;
config.insert(key, v);
}
if (itSrc != vs.end())
throw runtime_error("assembleConfig: input vector and ordering are not compatible with the graph");
return config;
}
/* ************************************************************************* */
Dimensions GaussianFactorGraph::columnIndices(const Ordering& ordering) const {

7
cpp/GaussianFactorGraph.h
View File

@ -175,6 +175,13 @@ namespace gtsam {
*/
std::pair<Matrix,Vector> matrix (const Ordering& ordering) const;
/**
* split the source vector w.r.t. the given ordering and assemble a vector config
* @param v: the source vector
* @param ordeirng: the ordering corresponding to the vector
*/
VectorConfig assembleConfig(const Vector& v, const Ordering& ordering) const;
/**
* get the starting column indices for all variables
* @param ordering of variables needed for matrix column order

17
cpp/Pose2SLAMOptimizer.cpp
View File

@ -37,8 +37,9 @@ namespace gtsam {
/* ************************************************************************* */
void Pose2SLAMOptimizer::update(const Vector& x) {
VectorConfig X; // TODO
update(X);
VectorConfig X = system_->assembleConfig(x, *solver_.ordering());
*theta_ = expmap(*theta_, X);
linearize();
}
/* ************************************************************************* */
@ -48,4 +49,16 @@ namespace gtsam {
}
/* ************************************************************************* */
Vector Pose2SLAMOptimizer::optimize() const {
VectorConfig X = solver_.optimize(*system_);
std::list<Vector> vs;
BOOST_FOREACH(const Symbol& key, *solver_.ordering())
vs.push_back(X[key]);
return concatVectors(vs);
}
/* ************************************************************************* */
double Pose2SLAMOptimizer::error() const {
return graph_->error(*theta_);
}
}

29
cpp/Pose2SLAMOptimizer.h
View File

@ -8,6 +8,8 @@
#ifndef POSE2SLAMOPTIMIZER_H_
#define POSE2SLAMOPTIMIZER_H_
#include <boost/foreach.hpp>
#include "pose2SLAM.h"
#include "Ordering.h"
#include "SubgraphPreconditioner.h"
@ -54,12 +56,14 @@ namespace gtsam {
boost::shared_ptr<const Pose2Graph> graph() const {
return graph_;
}
/**
* linearize around current theta
* return the current linearization point
*/
boost::shared_ptr<const Pose2Config> theta() const {
return theta_;
}
/**
* linearize around current theta
*/
@ -67,31 +71,22 @@ namespace gtsam {
system_ = solver_.linearize(*graph_, *theta_);
}
/**
* update estimate with pure delta config x
*/
void update(const VectorConfig& x) {
// TODO instead of assigning can we create a new one and replace the shared ptr ?
*theta_ = expmap(*theta_, x);
linearize();
}
/**
* Optimize to get a
*/
Vector optimize() {
VectorConfig X = solver_.optimize(*system_);
Vector x; // TODO convert to Vector
return x;
}
Vector optimize() const;
double error() const;
/**
* Return matrices associated with optimization problem
* around current non-linear estimate theta
* Returns [IJS] sparse representation
*/
Matrix Ab1() const { return system_->Ab1(*solver_.ordering()); }
Matrix Ab2() const { return system_->Ab2(*solver_.ordering()); }
Matrix a1() const { return system_->A1(*solver_.ordering()); }
Matrix a2() const { return system_->A2(*solver_.ordering()); }
Vector b1() const { return system_->b1(); }
Vector b2() const { return system_->b2(); }
/**
* update estimate with pure delta config x

7
cpp/SubgraphPreconditioner.h
View File

@ -44,8 +44,11 @@ namespace gtsam {
*/
SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2, sharedBayesNet& Rc1, sharedConfig& xbar);
Matrix Ab1(const Ordering& ordering) const { return Ab1_->sparse(ordering); }
Matrix Ab2(const Ordering& ordering) const { return Ab2_->sparse(ordering); }
Matrix A1(const Ordering& ordering) const { return Ab1_->sparse(ordering); }
Matrix A2(const Ordering& ordering) const { return Ab2_->sparse(ordering); }
Vector b1() const { return Ab1_->rhsVector(); }
Vector b2() const { return Ab2_->rhsVector(); }
VectorConfig assembleConfig(const Vector& v, const Ordering& ordering) const { return Ab1_->assembleConfig(v, ordering); }
/* x = xbar + inv(R1)*y */
VectorConfig x(const VectorConfig& y) const;

8
cpp/gtsam.h
View File

@ -1,8 +1,12 @@
class Pose2SLAMOptimizer {
Pose2SLAMOptimizer(string dataset_name);
Matrix Ab1() const;
Matrix Ab2() const;
void update(Vector x) const;
Vector optimize() const;
double error() const;
Matrix a1() const;
Matrix a2() const;
Vector b1() const;
Vector b2() const;
};
class SharedGaussian {

58
cpp/testPose2SLAM.cpp
View File

@ -162,25 +162,53 @@ TEST(Pose2Graph, optimizeCircle) {
// Check loop closure
CHECK(assert_equal(delta,between(actual[5],actual[0])));
// Try PCG class
// Pose2SLAMOptimizer myOptimizer("3");
// Matrix Ab1 = myOptimizer.Ab1();
// CHECK(assert_equal(Matrix_(1,1,1.0),Ab1));
// myOptimizer.linearize();
//
// Matrix A1 = myOptimizer.a1();
// LONGS_EQUAL(3, A1.size1());
// LONGS_EQUAL(17, A1.size2()); // 7 + 7 + 3
//
// Matrix A2 = myOptimizer.a2();
// LONGS_EQUAL(3, A1.size1());
// LONGS_EQUAL(7, A2.size2()); // 7
//
// Vector b1 = myOptimizer.b1();
// LONGS_EQUAL(9, b1.size()); // 3 + 3 + 3
//
// Vector b2 = myOptimizer.b2();
// LONGS_EQUAL(3, b2.size()); // 3
//
// // Here, call matlab to
// // A=[A1;A2], b=[b1;b2]
// // R=qr(A1)
// // call pcg on A,b, with preconditioner R -> get x
//
// Vector x = myOptimizer.optimize();
// LONGS_EQUAL(9, x.size()); // 3 + 3 + 3
//
// Matrix Ab2 = myOptimizer.Ab2();
// CHECK(assert_equal(Matrix_(1,1,1.0),Ab2));
// Here, call matlab to
// A=[A1;A2], b=[b1;b2]
// R=qr(A1)
// call pcg on A,b, with preconditioner R -> get x
// Vector x;
// myOptimizer.update(x);
//
// Pose2Config expected;
// expected.insert(0, Pose2(0.,0.,0.));
// expected.insert(1, Pose2(1.,0.,0.));
// expected.insert(2, Pose2(2.,0.,0.));
//
// // Check with ground truth
// CHECK(assert_equal(expected, *myOptimizer.theta()));
}
// Check with ground truth
// CHECK(assert_equal(hexagon, *myOptimizer.theta()));
TEST(Pose2Graph, optimize2) {
// Pose2SLAMOptimizer myOptimizer("100");
//
// //cout << "error: " << myOptimizer.error() << endl;
// for(int i = 0; i<10; i++) {
// myOptimizer.linearize();
// Vector x = myOptimizer.optimize();
// myOptimizer.update(x);
// }
// //cout << "error: " << myOptimizer.error() << endl;
// CHECK(myOptimizer.error() < 1.);
}
/* ************************************************************************* */