diff --git a/.cproject b/.cproject index fcf87c89d..8cc24a502 100644 --- a/.cproject +++ b/.cproject @@ -5,46 +5,46 @@ - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -298,522 +298,529 @@ - - -make --j2 -install -true -true -true - - -make --j2 -check -true -true -true - - -make --k -check -true -false -true - - -make --j2 -testSimpleCamera.run -true -true -true - - -make --f local.mk -testCal3_S2.run -true -true -true - - -make --j2 -testVSLAMFactor.run -true -true -true - - -make --j2 -testCalibratedCamera.run -true -true -true - - -make --j2 -testGaussianConditional.run -true -true -true - - -make --j2 -testPose2.run -true -true -true - - -make --j2 -testRot3.run -true -true -true - - -make --j2 -testNonlinearOptimizer.run -true -true -true - - -make --j2 -testGaussianFactor.run -true -true -true - - -make --j2 -testGaussianFactorGraph.run -true -true -true - - -make --j2 -testNonlinearFactorGraph.run -true -true -true - - -make --j2 -testPose3.run -true -true -true - - -make --j2 -testVectorConfig.run -true -true -true - - -make --j2 -testPoint2.run -true -true -true - - -make --j2 -testNonlinearFactor.run -true -true -true - - -make --j2 -timeGaussianFactor.run -true -true -true - - -make --j2 -timeGaussianFactorGraph.run -true -true -true - - -make --j2 -testGaussianBayesNet.run -true -true -true - - -make - -testBayesTree.run -true -false -true - - -make -testSymbolicBayesNet.run -true -false -true - - -make - -testSymbolicFactorGraph.run -true -false -true - - -make --j2 -testVector.run -true -true -true - - -make --j2 -testMatrix.run -true -true -true - - -make --j2 -testVSLAMGraph.run -true -true -true - - -make --j2 -testNonlinearEquality.run -true -true -true - - -make --j2 -testSQP.run -true -true -true - - -make --j2 -testNonlinearConstraint.run -true -true -true - - -make --j2 -testSQPOptimizer.run -true -true -true - - -make --j2 -testVSLAMConfig.run -true -true -true - - -make --j2 -testControlConfig.run -true -true -true - - -make --j2 -testControlPoint.run -true -true -true - - -make --j2 -testControlGraph.run -true -true -true - - -make --j2 -testOrdering.run -true -true -true - - -make --j2 -testRot2.run -true -true -true - - -make --j2 -testGaussianBayesTree.run -true -true -true - - -make --j2 -testPose3Config.run -true -true -true - - -make --j2 -testUrbanMeasurement.run -true -true -true - - -make --j2 -testUrbanOdometry.run -true -true -true - - -make --j2 -testIterative.run -true -true -true - - -make --j2 -testGaussianISAM2.run -true -true -true - - -make --j2 -testSubgraphPreconditioner.run -true -true -true - - -make --j2 -testBayesNetPreconditioner.run -true -true -true - - -make --j2 -testPose2Factor.run -true -true -true - - -make --j2 -timeRot3.run -true -true -true - - -make --j2 -testPose2SLAM.run -true -true -true - - -make --j2 -testPose2Config.run -true -true -true - - -make --j2 -testLieConfig.run -true -true -true - - -make --j2 -testPlanarSLAM.run -true -true -true - - -make - -testGraph.run -true -false -true - - -make --j2 -testPose3SLAM.run -true -true -true - - -make --j2 -testTupleConfig.run -true -true -true - - -make --j2 -testPose2Prior.run -true -true -true - - -make --j2 -testNoiseModel.run -true -true -true - - -make --j2 -testISAM.run -true -true -true - - -make --j2 -testGaussianISAM.run -true -true -true - - -make -testSimulated2D.run -true -false -true - - -make --j2 -timeMatrix.run -true -true -true - - -make --j2 -testKey.run -true -true -true - - -make --j2 -install -true -true -true - - -make --j2 -clean -true -true -true - - -make --j2 -check -true -true -true - - - - + + + make + -j2 + install + true + true + true + + + make + -j2 + check + true + true + true + + + make + -k + check + true + false + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -f local.mk + testCal3_S2.run + true + true + true + + + make + -j2 + testVSLAMFactor.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + testGaussianConditional.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testGaussianFactorGraph.run + true + true + true + + + make + -j2 + testNonlinearFactorGraph.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + testVectorConfig.run + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + + + make + -j2 + testNonlinearFactor.run + true + true + true + + + make + -j2 + timeGaussianFactor.run + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + testBayesTree.run + true + false + true + + + make + + testSymbolicBayesNet.run + true + false + true + + + make + testSymbolicFactorGraph.run + true + false + true + + + make + -j2 + testVector.run + true + true + true + + + make + -j2 + testMatrix.run + true + true + true + + + make + -j2 + testVSLAMGraph.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testSQP.run + true + true + true + + + make + -j2 + testNonlinearConstraint.run + true + true + true + + + make + -j2 + testSQPOptimizer.run + true + true + true + + + make + -j2 + testVSLAMConfig.run + true + true + true + + + make + -j2 + testControlConfig.run + true + true + true + + + make + -j2 + testControlPoint.run + true + true + true + + + make + -j2 + testControlGraph.run + true + true + true + + + make + -j2 + testOrdering.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testGaussianBayesTree.run + true + true + true + + + make + -j2 + testPose3Config.run + true + true + true + + + make + -j2 + testUrbanMeasurement.run + true + true + true + + + make + -j2 + testUrbanOdometry.run + true + true + true + + + make + -j2 + testIterative.run + true + true + true + + + make + -j2 + testGaussianISAM2.run + true + true + true + + + make + -j2 + testSubgraphPreconditioner.run + true + true + true + + + make + -j2 + testBayesNetPreconditioner.run + true + true + true + + + make + -j2 + testPose2Factor.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2SLAM.run + true + true + true + + + make + -j2 + testPose2Config.run + true + true + true + + + make + -j2 + testLieConfig.run + true + true + true + + + make + -j2 + testPlanarSLAM.run + true + true + true + + + make + testGraph.run + true + false + true + + + make + -j2 + testPose3SLAM.run + true + true + true + + + make + -j2 + testTupleConfig.run + true + true + true + + + make + -j2 + testPose2Prior.run + true + true + true + + + make + -j2 + testNoiseModel.run + true + true + true + + + make + -j2 + testISAM.run + true + true + true + + + make + -j2 + testGaussianISAM.run + true + true + true + + + make + + testSimulated2D.run + true + false + true + + + make + -j2 + timeMatrix.run + true + true + true + + + make + -j2 + testKey.run + true + true + true + + + make + -j2 + testPose2SLAM + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + + - - + + diff --git a/.project b/.project index c10203bc2..893b1174b 100644 --- a/.project +++ b/.project @@ -23,7 +23,7 @@ org.eclipse.cdt.make.core.buildArguments - + -j2 org.eclipse.cdt.make.core.buildCommand diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index cef54d4f1..480d648fc 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/cpp/GaussianFactorGraph.cpp @@ -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 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 { diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index 11135f7d3..509248c5d 100644 --- a/cpp/GaussianFactorGraph.h +++ b/cpp/GaussianFactorGraph.h @@ -175,6 +175,13 @@ namespace gtsam { */ std::pair 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 diff --git a/cpp/Pose2SLAMOptimizer.cpp b/cpp/Pose2SLAMOptimizer.cpp index f72515e59..c5b97feb9 100644 --- a/cpp/Pose2SLAMOptimizer.cpp +++ b/cpp/Pose2SLAMOptimizer.cpp @@ -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(); } /* ************************************************************************* */ @@ -47,5 +48,17 @@ namespace gtsam { update(x); } -/* ************************************************************************* */ + /* ************************************************************************* */ + Vector Pose2SLAMOptimizer::optimize() const { + VectorConfig X = solver_.optimize(*system_); + std::list vs; + BOOST_FOREACH(const Symbol& key, *solver_.ordering()) + vs.push_back(X[key]); + return concatVectors(vs); + } + + /* ************************************************************************* */ + double Pose2SLAMOptimizer::error() const { + return graph_->error(*theta_); + } } diff --git a/cpp/Pose2SLAMOptimizer.h b/cpp/Pose2SLAMOptimizer.h index 542016a75..08e5460ff 100644 --- a/cpp/Pose2SLAMOptimizer.h +++ b/cpp/Pose2SLAMOptimizer.h @@ -8,6 +8,8 @@ #ifndef POSE2SLAMOPTIMIZER_H_ #define POSE2SLAMOPTIMIZER_H_ +#include + #include "pose2SLAM.h" #include "Ordering.h" #include "SubgraphPreconditioner.h" @@ -54,12 +56,14 @@ namespace gtsam { boost::shared_ptr graph() const { return graph_; } + /** - * linearize around current theta + * return the current linearization point */ boost::shared_ptr 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 diff --git a/cpp/SubgraphPreconditioner.h b/cpp/SubgraphPreconditioner.h index 87b84a66a..d6675bbcf 100644 --- a/cpp/SubgraphPreconditioner.h +++ b/cpp/SubgraphPreconditioner.h @@ -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; diff --git a/cpp/gtsam.h b/cpp/gtsam.h index a93a2c2e2..c8cf91dea 100644 --- a/cpp/gtsam.h +++ b/cpp/gtsam.h @@ -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 { diff --git a/cpp/testPose2SLAM.cpp b/cpp/testPose2SLAM.cpp index cc2c8518e..ed60ffdf6 100644 --- a/cpp/testPose2SLAM.cpp +++ b/cpp/testPose2SLAM.cpp @@ -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.); } /* ************************************************************************* */