testDynamicValues passed

gtsam/base/DerivedValue.cpp

@@ -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 */

gtsam/base/LieVector.h

@@ -19,13 +19,14 @@
 
 #include <gtsam/base/Lie.h>
 #include <gtsam/base/Vector.h>
+#include <gtsam/base/DerivedValue.h>
 
 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<LieVector> {
 
 	/** default constructor - should be unnecessary */
 	LieVector() {}

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

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

gtsam/geometry/Cal3_S2Stereo.h

@@ -26,7 +26,7 @@ namespace gtsam {
 	 * @ingroup geometry
 	 * \nosubgrouping
 	 */
-	class Cal3_S2Stereo: public DerivedValue<Cal3_S2Stereo>, public Cal3_S2 {
+	class Cal3_S2Stereo: public Cal3_S2 {
 	private:
 		double 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) ;

gtsam/geometry/Point2.h

@@ -32,7 +32,7 @@ namespace gtsam {
  * @ingroup geometry
  * \nosubgrouping
  */
-class Point2 : DerivedValue<Point2> {
+class Point2 : public DerivedValue<Point2> {
 public:
 	/// dimension of the variable - used to autodetect sizes
 	static const size_t dimension = 2;

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<SimpleCamera> {
+	class SimpleCamera {
 	private:
 		CalibratedCamera calibrated_; // Calibrated camera
 		Cal3_S2 K_; // Calibration
@@ -77,6 +77,12 @@ namespace gtsam {
 			    boost::optional<Matrix&> H1 = boost::none,
 			    boost::optional<Matrix&> 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);
 		}

gtsam/nonlinear/DynamicValues-inl.h

@@ -93,28 +93,4 @@ namespace gtsam {
     return exists<typename TypedKey::Value>(symbol);
   }
 
-  /* ************************************************************************* */
-  template<class ValueType>
-  void DynamicValues::insert(const Symbol& j, const ValueType& val) {
-    std::pair<iterator,bool> insertResult = values_.insert(j, val);
-    if(!insertResult.second)
-      throw DynamicValuesKeyAlreadyExists(j);
-  }
-
-  /* ************************************************************************* */
-  template<class ValueType>
-  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<ValueType&>(*item->second);
-
-    // Assign
-    valueAsDerived = val;
-  }
 }

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<iterator,bool> 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;
   }

gtsam/nonlinear/DynamicValues.h

@@ -178,19 +178,17 @@ namespace gtsam {
     // imperative methods:
 
     /** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
-    template<class ValueType>
+    void insert(const Symbol& j, const Value& val);
-    void insert(const Symbol& j, const ValueType& val);
 
     /** Add a set of variables, throws KeyAlreadyExists<J> 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<class ValueType>
-    void update(const Symbol& j, const ValueType& val);
-
     /** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
     void erase(const Symbol& j);
 

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
 

gtsam/nonlinear/tests/testDynamicValues.cpp

@@ -22,6 +22,7 @@ using namespace boost::assign;
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/LieVector.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/DynamicValues.h>
 
 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);
+  LieVector v1(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, 5.0, 6.0, 8.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);
+  LieVector v1(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, inf, inf, inf);
+  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);
+  LieVector v1(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
+  LieVector v2(3, 8.0, 9.0, 1.0);
-  Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
+  LieVector v3(3, 2.0, 4.0, 3.0);
-  Vector v4 = Vector_(3, 8.0, 3.0, 7.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);
+  LieVector v1(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
+  LieVector v2(3, 8.0, 9.0, 1.0);
-  Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
+  LieVector v3(3, 2.0, 4.0, 3.0);
-  Vector v4 = Vector_(3, 8.0, 3.0, 7.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);
+  LieVector v1(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, 8.0, 9.0, 1.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(key1, LieVector(2, 2.0, 3.0));
-//  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+//  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
-//  LONGS_EQUAL(5,config0.dim());
+//  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(key1, LieVector(3, 1.0, 2.0, 3.0));
-  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.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(key1, LieVector(3, 2.0, 3.1, 4.2));
-  expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
+  expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
 
   CHECK(assert_equal(expected, config0.retract(increment, ordering)));
 }
 
-///* ************************************************************************* */
+/* ************************************************************************* */
-//TEST(DynamicValues, expmap_b)
+TEST(DynamicValues, expmap_b)
-//{
+{
-//  DynamicValues config0;
+  DynamicValues config0;
-//  config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
+  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
-//  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+  config0.insert(key2, LieVector(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)));
-//}
 
-///* ************************************************************************* */
+  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(key1, LieVector(3, 1.0, 2.0, 3.0));
-//  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.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(key1, LieVector(3, 2.0, 3.1, 4.2));
-//  expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
+//  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(key1, LieVector(3, 1.0, 2.0, 3.0));
-  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
   //config0.print("config0");
   CHECK(equal(config0, config0));
   CHECK(config0.equals(config0));
 
-  DynamicValues<string,Pose2> poseconfig;
+  typedef TypedSymbol<Pose2, 'p'> PoseKey;
-  poseconfig.insert("p1", Pose2(1,2,3));
+  DynamicValues poseconfig;
-  poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
+  poseconfig.insert(PoseKey(1), Pose2(1,2,3));
-  //poseconfig.print("poseconfig");
+  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<Pose2, 'x'> PoseKey;
-	DynamicValues<PoseKey, Pose2> config;
+	DynamicValues config;
 
 	config.insert(PoseKey(1), Pose2());
 	config.insert(PoseKey(2), Pose2());
 	config.insert(PoseKey(4), Pose2());
 	config.insert(PoseKey(5), Pose2());
 
-	list<PoseKey> expected, actual;
+	FastList<Symbol> expected, actual;
 	expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5);
 	actual = config.keys();
 
 	CHECK(actual.size() == expected.size());
-	list<PoseKey>::const_iterator itAct = actual.begin(), itExp = expected.begin();
+	FastList<Symbol>::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(key1, LieVector(Vector_(1, 1.)));
-	config0.insert(key2, Vector_(1, 2.));
+	config0.insert(key2, LieVector(Vector_(1, 2.)));
 
 	boost::optional<const LieVector&> 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(key1, LieVector(1, 1.));
-	config0.insert(key2, Vector_(1, 2.));
+	config0.insert(key2, LieVector(1, 2.));
 
 	DynamicValues superset;
-	superset.insert(key1, Vector_(1, -1.));
+	superset.insert(key1, LieVector(1, -1.));
-	superset.insert(key2, Vector_(1, -2.));
+	superset.insert(key2, LieVector(1, -2.));
-	superset.insert(key3, Vector_(1, -3.));
 	config0.update(superset);
 
 	DynamicValues expected;
-	expected.insert(key1, Vector_(1, -1.));
+	expected.insert(key1, LieVector(1, -1.));
-	expected.insert(key2, Vector_(1, -2.));
+	expected.insert(key2, LieVector(1, -2.));
 	CHECK(assert_equal(expected,config0));
 }
 
-/* ************************************************************************* */
-TEST(DynamicValues, dummy_initialization)
-{
-	typedef TypedSymbol<LieVector, 'z'> 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); }
 /* ************************************************************************* */