diff --git a/cpp/Pose3.h b/cpp/Pose3.h index b7fb2e5c4..7cc61bd1a 100644 --- a/cpp/Pose3.h +++ b/cpp/Pose3.h @@ -68,6 +68,9 @@ namespace gtsam { } }; // Pose3 class + // global print + inline void print(const Pose3& p, const std::string& s = "") { p.print(s);} + // Dimensionality of the tangent space inline size_t dim(const Pose3&) { return 6; } diff --git a/cpp/Pose3Factor.h b/cpp/Pose3Factor.h index 72aacd2a8..7c386438a 100644 --- a/cpp/Pose3Factor.h +++ b/cpp/Pose3Factor.h @@ -6,25 +6,16 @@ #pragma once #include -#include "BetweenFactor.h" #include "Pose3.h" +#include "LieConfig.h" +#include "BetweenFactor.h" namespace gtsam { /** * A config specifically for 3D poses */ - class Pose3Config: public std::map { - - public: - - const Pose3& get(std::string key) const { - const_iterator it = find(key); - if (it == end()) throw std::invalid_argument("invalid key"); - return it->second; - } - - }; + typedef LieConfig Pose3Config; /** * A Factor for 3D pose measurements diff --git a/cpp/testPose3Factor.cpp b/cpp/testPose3Factor.cpp index 26a384432..86948c8a4 100644 --- a/cpp/testPose3Factor.cpp +++ b/cpp/testPose3Factor.cpp @@ -9,6 +9,7 @@ using namespace boost::assign; #include #include "Pose3Factor.h" +#include "LieConfig-inl.h" using namespace std; using namespace gtsam; @@ -27,8 +28,8 @@ TEST( Pose3Factor, error ) // Create config Pose3Config x; - x.insert(make_pair("t1",t1)); - x.insert(make_pair("t2",t2)); + x.insert("t1",t1); + x.insert("t2",t2); // Get error z-h(x) -> logmap(h(x),z) = logmap(between(t1,t2),z) Vector actual = factor.error_vector(x); @@ -36,6 +37,13 @@ TEST( Pose3Factor, error ) CHECK(assert_equal(expected,actual)); } +/* ************************************************************************* */ +TEST( Pose3Factor, simple_circle ) +{ + Pose3Config expected, actual; + CHECK(assert_equal(expected,actual)); +} + /* ************************************************************************* */ int main() { TestResult tr;