diff --git a/cpp/TupleConfig.h b/cpp/TupleConfig.h index 05ca6da61..5e20f7d37 100644 --- a/cpp/TupleConfig.h +++ b/cpp/TupleConfig.h @@ -397,12 +397,11 @@ namespace gtsam { LieConfig first_; LieConfig second_; - private: + public: PairConfig(const LieConfig& config1, const LieConfig& config2) : first_(config1), second_(config2){} - public: /** * Default constructor creates an empty config. diff --git a/cpp/testTupleConfig.cpp b/cpp/testTupleConfig.cpp index cedcd8053..ff3807e3e 100644 --- a/cpp/testTupleConfig.cpp +++ b/cpp/testTupleConfig.cpp @@ -27,11 +27,33 @@ typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; typedef PairConfig Config; +/* ************************************************************************* */ +TEST( PairConfig, constructors ) +{ + Pose2 x1(1,2,3), x2(6,7,8); + Point2 l1(4,5), l2(9,10); + + Config::Config1 cfg1; + cfg1.insert(PoseKey(1), x1); + cfg1.insert(PoseKey(2), x2); + Config::Config2 cfg2; + cfg2.insert(PointKey(1), l1); + cfg2.insert(PointKey(2), l2); + + Config actual(cfg1, cfg2), expected; + expected.insert(PoseKey(1), x1); + expected.insert(PoseKey(2), x2); + expected.insert(PointKey(1), l1); + expected.insert(PointKey(2), l2); + + CHECK(assert_equal(expected, actual)); +} + /* ************************************************************************* */ TEST( PairConfig, insert_equals1 ) { - Pose2 x1(1,2,3), x2(6,7,8); - Point2 l1(4,5), l2(9,10); + Pose2 x1(1,2,3), x2(6,7,8); + Point2 l1(4,5), l2(9,10); Config expected; expected.insert(PoseKey(1), x1); @@ -48,7 +70,6 @@ TEST( PairConfig, insert_equals1 ) CHECK(assert_equal(expected,actual)); } -/* ************************************************************************* */ TEST( PairConfig, insert_equals2 ) { Pose2 x1(1,2,3), x2(6,7,8);