fixed a bug in update

cpp/LieConfig-inl.h

@@ -87,7 +87,7 @@ namespace gtsam {
   void LieConfig<J,T>::update(const LieConfig<J,T>& cfg) {
 	  BOOST_FOREACH(const typename Values::value_type& v, values_) {
 	  	boost::optional<T> t = cfg.exists_(v.first);
-	  	if (t) insert(v.first, *t);
+	  	if (t) values_[v.first] = *t;
 	  }
   }
 

cpp/testLieConfig.cpp

@@ -186,6 +186,35 @@ TEST(LieConfig, extract_keys)
 	}
 }
 
+/* ************************************************************************* */
+TEST(LieConfig, exists_)
+{
+	LieConfig<string,Vector> config0;
+	config0.insert("v1", Vector_(1, 1.));
+	config0.insert("v2", Vector_(1, 2.));
+
+	boost::optional<Vector> v = config0.exists_("v1");
+	CHECK(assert_equal(Vector_(1, 1.),*v));
+}
+
+/* ************************************************************************* */
+TEST(LieConfig, update)
+{
+	LieConfig<string,Vector> config0;
+	config0.insert("v1", Vector_(1, 1.));
+	config0.insert("v2", Vector_(1, 2.));
+
+	LieConfig<string,Vector> superset;
+	superset.insert("v1", Vector_(1, -1.));
+	superset.insert("v2", Vector_(1, -2.));
+	superset.insert("v3", Vector_(1, -3.));
+	config0.update(superset);
+
+	LieConfig<string,Vector> expected;
+	expected.insert("v1", Vector_(1, -1.));
+	expected.insert("v2", Vector_(1, -2.));
+	CHECK(assert_equal(expected,config0));
+}
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

cpp/testTupleConfig.cpp

@@ -484,6 +484,34 @@ TEST(TupleConfig, partial_insert) {
 	CHECK(assert_equal(expected, init));
 }
 
+/* ************************************************************************* */
+TEST(TupleConfig, update) {
+	TupleConfig3<PoseConfig, PointConfig, LamConfig> init, superset, expected;
+
+	PoseKey x1(1), x2(2);
+	PointKey l1(1), l2(2);
+	Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0);
+	Point2 point1(2.0, 3.0), point2(5.0, 6.0);
+
+	init.insert(x1, pose1);
+	init.insert(l1, point1);
+
+
+	Pose2 pose1_(1.0, 2.0, 0.4);
+	Point2 point1_(2.0, 4.0);
+	superset.insert(x1, pose1_);
+	superset.insert(l1, point1_);
+	superset.insert(x2, pose2);
+	superset.insert(l2, point2);
+	init.update(superset);
+
+	expected.insert(x1, pose1_);
+	expected.insert(l1, point1_);
+
+	CHECK(assert_equal(expected, init));
+}
+
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */