From 1a965340164a23ee26232f6716e0ae7c30371006 Mon Sep 17 00:00:00 2001 From: Viorela Ila Date: Thu, 10 Dec 2009 23:45:38 +0000 Subject: [PATCH] add Pose2Graph --- cpp/Makefile.am | 14 ++++++------ cpp/Pose2Config.h | 5 +++++ cpp/Pose2Factor.h | 18 ++++++++++++---- cpp/Pose2Graph.cpp | 25 ++++++++++++++++++++++ cpp/Pose2Graph.h | 47 +++++++++++++++++++++++++++++++++++++++++ cpp/gtsam.h | 9 ++++++++ cpp/testPose2Factor.cpp | 9 ++++++-- cpp/testPose2Graph.cpp | 36 +++++++++++++++++++++++++++++++ 8 files changed, 151 insertions(+), 12 deletions(-) create mode 100644 cpp/Pose2Graph.cpp create mode 100644 cpp/Pose2Graph.h create mode 100644 cpp/testPose2Graph.cpp diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 93bc41824..90701c957 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -125,11 +125,11 @@ headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h headers += SQPOptimizer.h SQPOptimizer-inl.h headers += NonlinearConstraint.h NonlinearConstraint-inl.h -headers += Pose2Config.h -headers += Pose2Factor.h +headers += NonlinearEquality.h +headers += Pose2Config.h Pose2Factor.h sources += NonlinearFactor.cpp -sources += NonlinearEquality.h -check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer testPose2Factor +sources += Pose2Graph.cpp +check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer testPose2Factor testPose2Graph testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp testNonlinearFactor_LDADD = libgtsam.la testNonlinearConstraint_SOURCES = $(example) testNonlinearConstraint.cpp @@ -144,8 +144,10 @@ testNonlinearEquality_SOURCES = testNonlinearEquality.cpp testNonlinearEquality_LDADD = libgtsam.la testSQP_SOURCES = $(example) testSQP.cpp testSQP_LDADD = libgtsam.la -testPose2Factor_SOURCES = $(example) testPose2Factor.cpp -testPose2Factor_LDADD = libgtsam.la +testPose2Factor_SOURCES = $(example) testPose2Factor.cpp +testPose2Factor_LDADD = libgtsam.la +testPose2Graph_SOURCES = $(example) testPose2Graph.cpp +testPose2Graph_LDADD = libgtsam.la # geometry sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp diff --git a/cpp/Pose2Config.h b/cpp/Pose2Config.h index 050bf5345..481aa09c5 100644 --- a/cpp/Pose2Config.h +++ b/cpp/Pose2Config.h @@ -1,3 +1,5 @@ +#pragma once + #include #include "Pose2.h" @@ -15,5 +17,8 @@ public: throw std::invalid_argument("invalid key"); return it->second; } + void insert(const std::string& name, const Pose2& val){ + std::map::insert(make_pair(name, val)); + } }; } // namespace diff --git a/cpp/Pose2Factor.h b/cpp/Pose2Factor.h index 6d14fe3c8..86783578b 100644 --- a/cpp/Pose2Factor.h +++ b/cpp/Pose2Factor.h @@ -2,8 +2,10 @@ * @file Pose2Factor.H * @authors Frank Dellaert, Viorela Ila **/ +#pragma once #include +#include "NonlinearFactor.h" #include "GaussianFactor.h" #include "VectorConfig.h" #include "Pose2.h" @@ -13,13 +15,16 @@ namespace gtsam { -class Pose2Factor : public Factor { +class Pose2Factor : public NonlinearFactor { private: std::string key1_, key2_; /** The keys of the two poses, order matters */ Pose2 measured_; Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */ public: + + typedef boost::shared_ptr shared_ptr; // shorthand for a smart pointer to a factor + Pose2Factor(const std::string& key1, const std::string& key2, const Pose2& measured, const Matrix& measurement_covariance): key1_(key1),key2_(key2),measured_(measured) { square_root_inverse_covariance_ = inverse_square_root(measurement_covariance); @@ -28,16 +33,21 @@ public: /** implement functions needed for Testable */ void print(const std::string& name) const { std::cout << name << std::endl; - std::cout << "Pose2Contraint"<< std::endl; + std::cout << "Factor "<< std::endl; std::cout << "key1 "<< key1_<& expected, double tol) const {return false;} + bool equals(const NonlinearFactor& expected, double tol) const {return false;} /** implement functions needed to derive from Factor */ - double error(const Pose2Config& c) const {return 0;} + Vector error_vector(const Pose2Config& config) const { + //z-h + Pose2 p1 = config.get(key1_), p2 = config.get(key2_); + return (measured_ - between(p1,p2)).vector(); + } + std::list keys() const { std::list l; return l; } std::size_t size() const { return 2;} diff --git a/cpp/Pose2Graph.cpp b/cpp/Pose2Graph.cpp new file mode 100644 index 000000000..4e054d7d4 --- /dev/null +++ b/cpp/Pose2Graph.cpp @@ -0,0 +1,25 @@ +/** + * @file Pose2Graph.cpp + * @brief A factor graph for the 2D PoseSLAM problem + * @authors Frank Dellaert, Viorela Ila + */ +//#include "NonlinearOptimizer-inl.h" +#include "FactorGraph-inl.h" +#include "NonlinearFactorGraph-inl.h" +#include "Pose2Graph.h" + +namespace gtsam { + +// explicit instantiation so all the code is there and we can link with it +template class FactorGraph > ; +template class NonlinearFactorGraph ; +//template class NonlinearOptimizer ; + +void Pose2Graph::print(const std::string& s) const { +} + +bool Pose2Graph::equals(const Pose2Graph& p, double tol) const { + return false; +} + +} // namespace gtsam diff --git a/cpp/Pose2Graph.h b/cpp/Pose2Graph.h new file mode 100644 index 000000000..4b4409c42 --- /dev/null +++ b/cpp/Pose2Graph.h @@ -0,0 +1,47 @@ +/** + * @file Pose2Graph.h + * @brief A factor graph for the 2D PoseSLAM problem + * @author Frank Dellaert + * @author Viorela Ila + */ + +#pragma once + + +#include "NonlinearFactorGraph.h" +#include "FactorGraph.h" +#include "Pose2Factor.h" +#include "Pose2Config.h" +#include "Testable.h" + +namespace gtsam{ + +/** + * Non-linear factor graph for visual SLAM + */ +class Pose2Graph : public gtsam::NonlinearFactorGraph{ + +public: + + /** default constructor is empty graph */ + Pose2Graph() {} + + /** + * print out graph + */ + void print(const std::string& s = "") const; + + /** + * equals + */ + bool equals(const Pose2Graph& p, double tol=1e-9) const; + + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) {} +}; + +} // namespace gtsam diff --git a/cpp/gtsam.h b/cpp/gtsam.h index dcde2ab38..821a8cec0 100644 --- a/cpp/gtsam.h +++ b/cpp/gtsam.h @@ -176,6 +176,7 @@ class Pose2{ class Pose2Config{ Pose2Config(); Pose2 get(string key) const; + void insert(string name, const Pose2& val); }; class Pose2Factor { @@ -187,3 +188,11 @@ class Pose2Factor { size_t size() const; GaussianFactor* linearize(const Pose2Config& config) const; }; + +class Pose2Graph{ + Pose2Graph(); + void print(string s) const; + bool equals(const Pose2Graph& p, double tol) const; + GaussianFactorGraph linearize(const Pose2Config& config) const; + void push_back(Pose2Factor* factor); +}; diff --git a/cpp/testPose2Factor.cpp b/cpp/testPose2Factor.cpp index 70191ec7e..3e1200255 100644 --- a/cpp/testPose2Factor.cpp +++ b/cpp/testPose2Factor.cpp @@ -9,6 +9,7 @@ #include #include "Pose2Factor.h" +#include "Pose2Graph.h" using namespace std; using namespace gtsam; @@ -29,13 +30,17 @@ TEST( Pose2Factor, constructor ) Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2) Pose2Config config; - config.insert(make_pair("p1",p1)); - config.insert(make_pair("p2",p2)); + config.insert("p1",p1); + config.insert("p2",p2); + //Pose2 pose1; + //pose1=config.get("p1"); + //pose1.print("pose1"); // Linearize boost::shared_ptr actual = constraint.linearize(config); + // expected Matrix expectedH1 = Matrix_(3,3, 0.0,-1.0,2.1, diff --git a/cpp/testPose2Graph.cpp b/cpp/testPose2Graph.cpp new file mode 100644 index 000000000..86ae8bb6b --- /dev/null +++ b/cpp/testPose2Graph.cpp @@ -0,0 +1,36 @@ + +/*STL/C++*/ +#include + +#include +#include "Pose2Graph.h" + +using namespace std; +using namespace gtsam; + + +TEST( Pose2Graph, constructor ) +{ + // create a factor between unknown poses p1 and p2 + Pose2 measured(2,2,M_PI_2); + Matrix measurement_covariance = Matrix_(3,3, + 0.25, 0.0, 0.0, + 0.0, 0.25, 0.0, + 0.0, 0.0, 0.01 + ); + Pose2Factor constraint("p1","p2",measured, measurement_covariance); + Pose2Graph graph; + graph.push_back(Pose2Factor::shared_ptr(new Pose2Factor("p1","p2",measured, measurement_covariance))); + // get the size of the graph + size_t actual = graph.size(); + // verify + size_t expected = 1; + CHECK(actual == expected); + +} +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */