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

fixed Ab2 bug

release/4.3a0
Kai Ni 2010-01-23 05:29:04 +00:00
parent afa964b8db
commit 1685920d52
4 changed files with 36 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -784,14 +784,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>

7
cpp/GaussianFactorGraph.cpp
View File

@ -255,15 +255,12 @@ Dimensions GaussianFactorGraph::columnIndices(const Ordering& ordering) const {
j += it->second; j += it->second;
} }
return result;} return result;
}
/* ************************************************************************* */ /* ************************************************************************* */
Matrix GaussianFactorGraph::sparse(const Ordering& ordering) const { Matrix GaussianFactorGraph::sparse(const Ordering& ordering) const {
// return values
list<int> I,J;
list<double> S;
// get the starting column indices for all variables // get the starting column indices for all variables
Dimensions indices = columnIndices(ordering); Dimensions indices = columnIndices(ordering);

2
cpp/SubgraphPreconditioner.h
View File

@ -47,7 +47,7 @@ namespace gtsam {
std::pair<Matrix,Vector> Ab1(const Ordering& ordering) const { return Ab1_->matrix(ordering); } std::pair<Matrix,Vector> Ab1(const Ordering& ordering) const { return Ab1_->matrix(ordering); }
std::pair<Matrix,Vector> Ab2(const Ordering& ordering) const { return Ab2_->matrix(ordering); } std::pair<Matrix,Vector> Ab2(const Ordering& ordering) const { return Ab2_->matrix(ordering); }
Matrix A1(const Ordering& ordering) const { return Ab1_->sparse(ordering); } Matrix A1(const Ordering& ordering) const { return Ab1_->sparse(ordering); }
Matrix A2(const Ordering& ordering) const { return Ab2_->sparse(ordering); } Matrix A2(const Ordering& ordering) const { return Ab2_->sparse(Ab1_->columnIndices(ordering)); }
Vector b1() const { return Ab1_->rhsVector(); } Vector b1() const { return Ab1_->rhsVector(); }
Vector b2() const { return Ab2_->rhsVector(); } Vector b2() const { return Ab2_->rhsVector(); }
VectorConfig assembleConfig(const Vector& v, const Ordering& ordering) const { return Ab1_->assembleConfig(v, ordering); } VectorConfig assembleConfig(const Vector& v, const Ordering& ordering) const { return Ab1_->assembleConfig(v, ordering); }

77
cpp/testPose2SLAM.cpp
View File

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