From afa964b8dbed5d1bbf1fc65f39893bb677257a74 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 23 Jan 2010 05:16:29 +0000 Subject: [PATCH] Fixed sparse bug --- cpp/GaussianFactorGraph.cpp | 15 +++++++- cpp/GaussianFactorGraph.h | 6 +++ cpp/Pose2SLAMOptimizer.h | 2 + cpp/SubgraphPreconditioner.h | 2 + cpp/testPose2SLAM.cpp | 75 +++++++++++++++++++++--------------- 5 files changed, 68 insertions(+), 32 deletions(-) diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index 480d648fc..ff7fde4bb 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/cpp/GaussianFactorGraph.cpp @@ -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 I,J; + list S; + // Collect the I,J,S lists for all factors int row_index = 0; BOOST_FOREACH(sharedFactor factor,factors_) { diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index 509248c5d..0f70fe19b 100644 --- a/cpp/GaussianFactorGraph.h +++ b/cpp/GaussianFactorGraph.h @@ -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 diff --git a/cpp/Pose2SLAMOptimizer.h b/cpp/Pose2SLAMOptimizer.h index c7fc9b7b8..70720bd9d 100644 --- a/cpp/Pose2SLAMOptimizer.h +++ b/cpp/Pose2SLAMOptimizer.h @@ -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 Ab1() const { return system_->Ab1(*solver_.ordering()); } + std::pair Ab2() const { return system_->Ab2(*solver_.ordering()); } /** * update estimate with pure delta config x diff --git a/cpp/SubgraphPreconditioner.h b/cpp/SubgraphPreconditioner.h index d6675bbcf..6e1a6c553 100644 --- a/cpp/SubgraphPreconditioner.h +++ b/cpp/SubgraphPreconditioner.h @@ -44,6 +44,8 @@ namespace gtsam { */ SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2, sharedBayesNet& Rc1, sharedConfig& xbar); + std::pair Ab1(const Ordering& ordering) const { return Ab1_->matrix(ordering); } + std::pair 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(); } diff --git a/cpp/testPose2SLAM.cpp b/cpp/testPose2SLAM.cpp index ed60ffdf6..346eb8d0d 100644 --- a/cpp/testPose2SLAM.cpp +++ b/cpp/testPose2SLAM.cpp @@ -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.); } /* ************************************************************************* */