From 79a3855316e9bd0de072b6704ecc91d9f3180259 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Fri, 27 Jan 2012 21:48:23 +0000 Subject: [PATCH] testDynamicValues passed --- gtsam/base/DerivedValue.cpp | 21 --- gtsam/base/LieVector.h | 3 +- gtsam/geometry/Cal3Bundler.h | 2 +- gtsam/geometry/Cal3DS2.h | 2 +- gtsam/geometry/Cal3_S2Stereo.h | 2 +- gtsam/geometry/CalibratedCamera.h | 5 + gtsam/geometry/Point2.h | 2 +- gtsam/geometry/SimpleCamera.h | 8 +- gtsam/nonlinear/DynamicValues-inl.h | 24 --- gtsam/nonlinear/DynamicValues.cpp | 29 +++- gtsam/nonlinear/DynamicValues.h | 10 +- gtsam/nonlinear/Makefile.am | 2 +- gtsam/nonlinear/tests/testDynamicValues.cpp | 157 +++++++++----------- 13 files changed, 117 insertions(+), 150 deletions(-) delete mode 100644 gtsam/base/DerivedValue.cpp diff --git a/gtsam/base/DerivedValue.cpp b/gtsam/base/DerivedValue.cpp deleted file mode 100644 index 386919d6f..000000000 --- a/gtsam/base/DerivedValue.cpp +++ /dev/null @@ -1,21 +0,0 @@ -/* - * DerivedValue.cpp - * - * Created on: Jan 26, 2012 - * Author: thduynguyen - */ - -#include "DerivedValue.h" - -namespace gtsam { - -DerivedValue::DerivedValue() { - // TODO Auto-generated constructor stub - -} - -DerivedValue::~DerivedValue() { - // TODO Auto-generated destructor stub -} - -} /* namespace gtsam */ diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index 5a1b3d580..6a6b3408d 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -19,13 +19,14 @@ #include #include +#include namespace gtsam { /** * LieVector is a wrapper around vector to allow it to be a Lie type */ -struct LieVector : public Vector { +struct LieVector : public Vector, public DerivedValue { /** default constructor - should be unnecessary */ LieVector() {} diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 9e7b4c327..65dd1a19a 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -95,7 +95,7 @@ public: Vector localCoordinates(const Cal3Bundler& T2) const ; ///TODO: comment - int dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + virtual size_t dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) ///TODO: comment static size_t Dim() { return 3; } //TODO: make a final dimension variable diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 86a397daa..0044f1c35 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -98,7 +98,7 @@ public: Vector localCoordinates(const Cal3DS2& T2) const ; ///TODO: comment - int dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + virtual size_t dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) ///TODO: comment static size_t Dim() { return 9; } //TODO: make a final dimension variable diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 31265fbac..a8366e35f 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -26,7 +26,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ - class Cal3_S2Stereo: public DerivedValue, public Cal3_S2 { + class Cal3_S2Stereo: public Cal3_S2 { private: double b_; diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index f1e647046..7c98d522e 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -62,6 +62,11 @@ namespace gtsam { /// @name Testable /// @{ + virtual void print(const std::string& s = "") const { + std::cout << s; + pose_.print("pose: "); + } + /// check equality to another camera bool equals (const CalibratedCamera &camera, double tol = 1e-9) const { return pose_.equals(camera.pose(), tol) ; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 4a8be8edf..45b9de308 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -32,7 +32,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ -class Point2 : DerivedValue { +class Point2 : public DerivedValue { public: /// dimension of the variable - used to autodetect sizes static const size_t dimension = 2; diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index ae80ad96e..2763d6a95 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -30,7 +30,7 @@ namespace gtsam { * to produce measurements in pixels. * Not a sublass as a SimpleCamera *is not* a CalibratedCamera. */ - class SimpleCamera : public DerivedValue { + class SimpleCamera { private: CalibratedCamera calibrated_; // Calibrated camera Cal3_S2 K_; // Calibration @@ -77,6 +77,12 @@ namespace gtsam { boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; + virtual void print(const std::string& s = "") const { + std::cout << s; + calibrated_.print("calibrated: "); + K_.print("K: "); + } + bool equals(const SimpleCamera& X, double tol=1e-9) const { return calibrated_.equals(X.calibrated_, tol) && K_.equals(X.K_, tol); } diff --git a/gtsam/nonlinear/DynamicValues-inl.h b/gtsam/nonlinear/DynamicValues-inl.h index 3c30b7c41..7686c2293 100644 --- a/gtsam/nonlinear/DynamicValues-inl.h +++ b/gtsam/nonlinear/DynamicValues-inl.h @@ -93,28 +93,4 @@ namespace gtsam { return exists(symbol); } - /* ************************************************************************* */ - template - void DynamicValues::insert(const Symbol& j, const ValueType& val) { - std::pair insertResult = values_.insert(j, val); - if(!insertResult.second) - throw DynamicValuesKeyAlreadyExists(j); - } - - /* ************************************************************************* */ - template - void DynamicValues::update(const Symbol& j, const ValueType& val) { - // Find the value to update - iterator item = values_.find(j); - if(item == values_.end()) - throw DynamicValuesKeyDoesNotExist("update", j); - - // Cast to the derived type - if(typeid(*item->second) != typeid(Value)) - throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(Value)); - ValueType& valueAsDerived = static_cast(*item->second); - - // Assign - valueAsDerived = val; - } } diff --git a/gtsam/nonlinear/DynamicValues.cpp b/gtsam/nonlinear/DynamicValues.cpp index c77894666..5b8d89a83 100644 --- a/gtsam/nonlinear/DynamicValues.cpp +++ b/gtsam/nonlinear/DynamicValues.cpp @@ -82,7 +82,6 @@ namespace gtsam { Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument Value* retractedValue(key_value->second->retract_(singleDelta)); // Retract result.values_.insert(key, retractedValue); // Add retracted result directly to result values - retractedValue->deallocate_(); } return result; @@ -107,14 +106,36 @@ namespace gtsam { } } + /* ************************************************************************* */ + void DynamicValues::insert(const Symbol& j, const Value& val) { + Symbol key = j; // Non-const duplicate to deal with non-const insert argument + std::pair insertResult = values_.insert(key, val.clone_()); + if(!insertResult.second) + throw DynamicValuesKeyAlreadyExists(j); + } + /* ************************************************************************* */ void DynamicValues::insert(const DynamicValues& values) { - for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) { + for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument - values_.insert(key, key_value->second->clone_()); + insert(key, *key_value->second); } } + /* ************************************************************************* */ + void DynamicValues::update(const Symbol& j, const Value& val) { + // Find the value to update + iterator item = values_.find(j); + if(item == values_.end()) + throw DynamicValuesKeyDoesNotExist("update", j); + + // Cast to the derived type + if(typeid(*item->second) != typeid(val)) + throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(val)); + + values_.replace(item, val.clone_()); + } + /* ************************************************************************* */ void DynamicValues::update(const DynamicValues& values) { for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { @@ -152,7 +173,7 @@ namespace gtsam { result.assign( boost::make_transform_iterator(values_.begin(), boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))), - boost::make_transform_iterator(values_.begin(), + boost::make_transform_iterator(values_.end(), boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1)))); return result; } diff --git a/gtsam/nonlinear/DynamicValues.h b/gtsam/nonlinear/DynamicValues.h index de9a42b84..7b19c963f 100644 --- a/gtsam/nonlinear/DynamicValues.h +++ b/gtsam/nonlinear/DynamicValues.h @@ -178,19 +178,17 @@ namespace gtsam { // imperative methods: /** Add a variable with the given j, throws KeyAlreadyExists if j is already present */ - template - void insert(const Symbol& j, const ValueType& val); + void insert(const Symbol& j, const Value& val); /** Add a set of variables, throws KeyAlreadyExists if a key is already present */ void insert(const DynamicValues& values); + /** single element change of existing element */ + void update(const Symbol& j, const Value& val); + /** update the current available values without adding new ones */ void update(const DynamicValues& values); - /** single element change of existing element */ - template - void update(const Symbol& j, const ValueType& val); - /** Remove a variable from the config, throws KeyDoesNotExist if j is not present */ void erase(const Symbol& j); diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index defcd51e4..f5e448a31 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -63,7 +63,7 @@ AM_CXXFLAGS = #---------------------------------------------------------------------------------------------------- TESTS = $(check_PROGRAMS) AM_LDFLAGS += $(boost_serialization) -LDADD = libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../base/libbase.la ../3rdparty/libccolamd.la +LDADD = libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../geometry/libgeometry.la ../base/libbase.la ../3rdparty/libccolamd.la LDADD += ../../CppUnitLite/libCppUnitLite.a AM_DEFAULT_SOURCE_EXT = .cpp diff --git a/gtsam/nonlinear/tests/testDynamicValues.cpp b/gtsam/nonlinear/tests/testDynamicValues.cpp index cd76389ec..f7493f1b1 100644 --- a/gtsam/nonlinear/tests/testDynamicValues.cpp +++ b/gtsam/nonlinear/tests/testDynamicValues.cpp @@ -22,6 +22,7 @@ using namespace boost::assign; #include #include +#include #include using namespace gtsam; @@ -36,7 +37,7 @@ VecKey key1(1), key2(2), key3(3), key4(4); TEST( DynamicValues, equals1 ) { DynamicValues expected; - Vector v = Vector_(3, 5.0, 6.0, 7.0); + LieVector v(3, 5.0, 6.0, 7.0); expected.insert(key1,v); DynamicValues actual; actual.insert(key1,v); @@ -47,8 +48,8 @@ TEST( DynamicValues, equals1 ) TEST( DynamicValues, equals2 ) { DynamicValues cfg1, cfg2; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, 5.0, 6.0, 8.0); + LieVector v1(3, 5.0, 6.0, 7.0); + LieVector v2(3, 5.0, 6.0, 8.0); cfg1.insert(key1, v1); cfg2.insert(key1, v2); CHECK(!cfg1.equals(cfg2)); @@ -59,8 +60,8 @@ TEST( DynamicValues, equals2 ) TEST( DynamicValues, equals_nan ) { DynamicValues cfg1, cfg2; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, inf, inf, inf); + LieVector v1(3, 5.0, 6.0, 7.0); + LieVector v2(3, inf, inf, inf); cfg1.insert(key1, v1); cfg2.insert(key1, v2); CHECK(!cfg1.equals(cfg2)); @@ -71,10 +72,10 @@ TEST( DynamicValues, equals_nan ) TEST( DynamicValues, insert_good ) { DynamicValues cfg1, cfg2, expected; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, 8.0, 9.0, 1.0); - Vector v3 = Vector_(3, 2.0, 4.0, 3.0); - Vector v4 = Vector_(3, 8.0, 3.0, 7.0); + LieVector v1(3, 5.0, 6.0, 7.0); + LieVector v2(3, 8.0, 9.0, 1.0); + LieVector v3(3, 2.0, 4.0, 3.0); + LieVector v4(3, 8.0, 3.0, 7.0); cfg1.insert(key1, v1); cfg1.insert(key2, v2); cfg2.insert(key3, v4); @@ -85,17 +86,17 @@ TEST( DynamicValues, insert_good ) expected.insert(key2, v2); expected.insert(key3, v4); - CHECK(assert_equal(cfg1, expected)); + CHECK(assert_equal(expected, cfg1)); } /* ************************************************************************* */ TEST( DynamicValues, insert_bad ) { DynamicValues cfg1, cfg2; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, 8.0, 9.0, 1.0); - Vector v3 = Vector_(3, 2.0, 4.0, 3.0); - Vector v4 = Vector_(3, 8.0, 3.0, 7.0); + LieVector v1(3, 5.0, 6.0, 7.0); + LieVector v2(3, 8.0, 9.0, 1.0); + LieVector v3(3, 2.0, 4.0, 3.0); + LieVector v4(3, 8.0, 3.0, 7.0); cfg1.insert(key1, v1); cfg1.insert(key2, v2); cfg2.insert(key2, v3); @@ -108,8 +109,8 @@ TEST( DynamicValues, insert_bad ) TEST( DynamicValues, update_element ) { DynamicValues cfg; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, 8.0, 9.0, 1.0); + LieVector v1(3, 5.0, 6.0, 7.0); + LieVector v2(3, 8.0, 9.0, 1.0); cfg.insert(key1, v1); CHECK(cfg.size() == 1); @@ -124,9 +125,9 @@ TEST( DynamicValues, update_element ) //TEST(DynamicValues, dim_zero) //{ // DynamicValues config0; -// config0.insert(key1, Vector_(2, 2.0, 3.0)); -// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); -// LONGS_EQUAL(5,config0.dim()); +// config0.insert(key1, LieVector(2, 2.0, 3.0)); +// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); +// LONGS_EQUAL(5, config0.dim()); // // VectorValues expected; // expected.insert(key1, zero(2)); @@ -138,8 +139,8 @@ TEST( DynamicValues, update_element ) TEST(DynamicValues, expmap_a) { DynamicValues config0; - config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); - config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); + config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); + config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); Ordering ordering(*config0.orderingArbitrary()); VectorValues increment(config0.dims(ordering)); @@ -147,94 +148,95 @@ TEST(DynamicValues, expmap_a) increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5); DynamicValues expected; - expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); - expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); + expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); + expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); CHECK(assert_equal(expected, config0.retract(increment, ordering))); } -///* ************************************************************************* */ -//TEST(DynamicValues, expmap_b) -//{ -// DynamicValues config0; -// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); -// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); -// -// Ordering ordering(*config0.orderingArbitrary()); -// VectorValues increment(config0.dims(ordering)); -// increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5); -// -// DynamicValues expected; -// expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); -// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); -// -// CHECK(assert_equal(expected, config0.retract(increment, ordering))); -//} +/* ************************************************************************* */ +TEST(DynamicValues, expmap_b) +{ + DynamicValues config0; + config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); + config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); -///* ************************************************************************* */ + Ordering ordering(*config0.orderingArbitrary()); + VectorValues increment(config0.dims(ordering)); + increment[ordering[key2]] = LieVector(3, 1.3, 1.4, 1.5); + + DynamicValues expected; + expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); + expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); + + CHECK(assert_equal(expected, config0.retract(increment, ordering))); +} + +/* ************************************************************************* */ //TEST(DynamicValues, expmap_c) //{ // DynamicValues config0; -// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); -// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); +// config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); +// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); // -// Vector increment = Vector_(6, +// Vector increment = LieVector(6, // 1.0, 1.1, 1.2, // 1.3, 1.4, 1.5); // // DynamicValues expected; -// expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); -// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); +// expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); +// expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); // // CHECK(assert_equal(expected, config0.retract(increment))); //} /* ************************************************************************* */ -/*TEST(DynamicValues, expmap_d) +TEST(DynamicValues, expmap_d) { DynamicValues config0; - config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); - config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); + config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); + config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); //config0.print("config0"); CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); - DynamicValues poseconfig; - poseconfig.insert("p1", Pose2(1,2,3)); - poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5)); - //poseconfig.print("poseconfig"); + typedef TypedSymbol PoseKey; + DynamicValues poseconfig; + poseconfig.insert(PoseKey(1), Pose2(1,2,3)); + poseconfig.insert(PoseKey(2), Pose2(0.3, 0.4, 0.5)); + CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); -}*/ +} /* ************************************************************************* */ -/*TEST(DynamicValues, extract_keys) +TEST(DynamicValues, extract_keys) { typedef TypedSymbol PoseKey; - DynamicValues config; + DynamicValues config; config.insert(PoseKey(1), Pose2()); config.insert(PoseKey(2), Pose2()); config.insert(PoseKey(4), Pose2()); config.insert(PoseKey(5), Pose2()); - list expected, actual; + FastList expected, actual; expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5); actual = config.keys(); CHECK(actual.size() == expected.size()); - list::const_iterator itAct = actual.begin(), itExp = expected.begin(); + FastList::const_iterator itAct = actual.begin(), itExp = expected.begin(); for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) { CHECK(assert_equal(*itExp, *itAct)); } -}*/ +} /* ************************************************************************* */ TEST(DynamicValues, exists_) { DynamicValues config0; - config0.insert(key1, Vector_(1, 1.)); - config0.insert(key2, Vector_(1, 2.)); + config0.insert(key1, LieVector(Vector_(1, 1.))); + config0.insert(key2, LieVector(Vector_(1, 2.))); boost::optional v = config0.exists(key1); CHECK(assert_equal(Vector_(1, 1.),*v)); @@ -244,41 +246,20 @@ TEST(DynamicValues, exists_) TEST(DynamicValues, update) { DynamicValues config0; - config0.insert(key1, Vector_(1, 1.)); - config0.insert(key2, Vector_(1, 2.)); + config0.insert(key1, LieVector(1, 1.)); + config0.insert(key2, LieVector(1, 2.)); DynamicValues superset; - superset.insert(key1, Vector_(1, -1.)); - superset.insert(key2, Vector_(1, -2.)); - superset.insert(key3, Vector_(1, -3.)); + superset.insert(key1, LieVector(1, -1.)); + superset.insert(key2, LieVector(1, -2.)); config0.update(superset); DynamicValues expected; - expected.insert(key1, Vector_(1, -1.)); - expected.insert(key2, Vector_(1, -2.)); + expected.insert(key1, LieVector(1, -1.)); + expected.insert(key2, LieVector(1, -2.)); CHECK(assert_equal(expected,config0)); } -/* ************************************************************************* */ -TEST(DynamicValues, dummy_initialization) -{ - typedef TypedSymbol Key; - - DynamicValues init1; - init1.insert(Key(1), Vector_(2, 1.0, 2.0)); - init1.insert(Key(2), Vector_(2, 4.0, 3.0)); - - DynamicValues init2; - init2.insert(key1, Vector_(2, 1.0, 2.0)); - init2.insert(key2, Vector_(2, 4.0, 3.0)); - - DynamicValues actual1(init2); - DynamicValues actual2(init1); - - EXPECT(actual1.empty()); - EXPECT(actual2.empty()); -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */