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

Fixed sparse bug

release/4.3a0
Frank Dellaert 2010-01-23 05:16:29 +00:00
parent 744269343f
commit afa964b8db
5 changed files with 68 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
cpp/GaussianFactorGraph.cpp
View File

@ -249,12 +249,13 @@ Dimensions GaussianFactorGraph::columnIndices(const Ordering& ordering) const {
result.insert(make_pair(key,j));
// find dimension for this key
Dimensions::const_iterator it = variableSet.find(key);
if (it==variableSet.end()) // key not found, now what ?
throw invalid_argument("ColumnIndices: this ordering contains keys not in the graph");
// advance column index to next block by adding dim(key)
j += it->second;
}
return result;
}
return result;}
/* ************************************************************************* */
Matrix GaussianFactorGraph::sparse(const Ordering& ordering) const {
@ -266,6 +267,16 @@ Matrix GaussianFactorGraph::sparse(const Ordering& ordering) const {
// get the starting column indices for all variables
Dimensions indices = columnIndices(ordering);
return sparse(indices);
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::sparse(const Dimensions& indices) const {
// return values
list<int> I,J;
list<double> S;
// Collect the I,J,S lists for all factors
int row_index = 0;
BOOST_FOREACH(sharedFactor factor,factors_) {

6
cpp/GaussianFactorGraph.h
View File

@ -197,6 +197,12 @@ namespace gtsam {
*/
Matrix sparse(const Ordering& ordering) const;
/**
* Version that takes column indices rather than ordering
*/
Matrix sparse(const Dimensions& indices) const;
/**
* Take an optimal step in direction d by calculating optimal step-size
* @param x: starting point for search

2
cpp/Pose2SLAMOptimizer.h
View File

@ -92,6 +92,8 @@ namespace gtsam {
Matrix a2() const { return system_->A2(*solver_.ordering()); }
Vector b1() const { return system_->b1(); }
Vector b2() const { return system_->b2(); }
std::pair<Matrix,Vector> Ab1() const { return system_->Ab1(*solver_.ordering()); }
std::pair<Matrix,Vector> Ab2() const { return system_->Ab2(*solver_.ordering()); }
/**
* update estimate with pure delta config x

2
cpp/SubgraphPreconditioner.h
View File

@ -44,6 +44,8 @@ namespace gtsam {
*/
SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2, sharedBayesNet& Rc1, sharedConfig& xbar);
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); }
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(); }

75
cpp/testPose2SLAM.cpp
View File

@ -162,9 +162,19 @@ TEST(Pose2Graph, optimizeCircle) {
// Check loop closure
CHECK(assert_equal(delta,between(actual[5],actual[0])));
// Pose2SLAMOptimizer myOptimizer("3");
// myOptimizer.linearize();
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();
// LONGS_EQUAL(3, A1.size1());
// LONGS_EQUAL(17, A1.size2()); // 7 + 7 + 3
@ -178,37 +188,42 @@ TEST(Pose2Graph, optimizeCircle) {
//
// 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
//
// 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()));
// 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
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()));
}
/* ************************************************************************* */
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.);
Pose2SLAMOptimizer myOptimizer("100");
Matrix A1 = myOptimizer.a1();
Matrix A2 = myOptimizer.a2();
cout << "A1: " << A1.size1() << " " << A1.size2() << endl;
cout << "A2: " << A2.size1() << " " << A2.size2() << endl;
//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.);
}
/* ************************************************************************* */