(in branch) Fixed Value base class assignment operator, ISAM2 expmap function, and unit tests

gtsam/base/DerivedValue.h

@@ -73,6 +73,23 @@ public:
     return (static_cast<const DERIVED*>(this))->localCoordinates(derivedValue2);
 	}
 
+	/// Assignment operator
+	virtual Value& operator=(const Value& rhs) {
+    // Cast the base class Value pointer to a derived class pointer
+    const DERIVED& derivedRhs = dynamic_cast<const DERIVED&>(rhs);
+
+    // Do the assignment and return the result
+    return (static_cast<DERIVED*>(this))->operator=(derivedRhs);
+	}
+
+protected:
+	/// Assignment operator, protected because only the Value or DERIVED
+	/// assignment operators should be used.
+	DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& rhs) {
+	  // Nothing to do, do not call base class assignment operator
+	  return *this;
+	}
+
 };
 
 } /* namespace gtsam */

gtsam/base/Value.h

@@ -136,6 +136,9 @@ namespace gtsam {
      */
     virtual Vector localCoordinates_(const Value& value) const = 0;
 
+    /** Assignment operator */
+    virtual Value& operator=(const Value& rhs) = 0;
+
     /** Virutal destructor */
     virtual ~Value() {}
 

gtsam/linear/VectorValues.h

@@ -175,7 +175,7 @@ namespace gtsam {
     /// @name Advanced Constructors
     /// @{
 
-    /** Construct from a container of variable dimensions (in variable order). */
+    /** Construct from a container of variable dimensions (in variable order), without initializing any values. */
     template<class CONTAINER>
     VectorValues(const CONTAINER& dimensions) { append(dimensions); }
 

gtsam/nonlinear/ISAM2-impl-inl.h

@@ -245,10 +245,12 @@ struct _SelectiveExpmapAndClear {
       else
         cout << "       " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
     }
-    assert(delta[var].size() == (int)it_x->second.dim());
+    assert(delta[var].size() == (int)it_x->second->dim());
     assert(delta[var].unaryExpr(&isfinite<double>).all());
     if(mask[var]) {
-      it_x->second = it_x->second.retract(delta[var]);
+      Value* retracted = it_x->second->retract_(delta[var]);
+      *it_x->second = *retracted;
+      retracted->deallocate_();
       if(invalidate)
         (*invalidate)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?)
     }

gtsam/nonlinear/tests/testDynamicValues.cpp

@@ -162,7 +162,7 @@ TEST(DynamicValues, expmap_b)
   config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
 
   Ordering ordering(*config0.orderingArbitrary());
-  VectorValues increment(config0.dims(ordering));
+  VectorValues increment(VectorValues::Zero(config0.dims(ordering)));
   increment[ordering[key2]] = LieVector(3, 1.3, 1.4, 1.5);
 
   DynamicValues expected;

gtsam/slam/tests/testProjectionFactor.cpp

@@ -85,8 +85,6 @@ TEST( ProjectionFactor, error )
   Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(PoseKey(1), x2);
   Point3 l2(1,2,3); expected_config.insert(PointKey(1), l2);
 	VectorValues delta(expected_config.dims(ordering));
-	ordering.print("ordering: ");
-	delta.print("delta: ");
 	delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.);
 	delta[ordering["l1"]] = Vector_(3, 1.,2.,3.);
 	DynamicValues actual_config = config.retract(delta, ordering);

tests/Makefile.am

@@ -20,7 +20,7 @@ check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph
 check_PROGRAMS += testNonlinearISAM
 check_PROGRAMS += testBoundingConstraint
 #check_PROGRAMS += testPose2SLAMwSPCG
-#check_PROGRAMS += testGaussianISAM2
+check_PROGRAMS += testGaussianISAM2
 check_PROGRAMS += testExtendedKalmanFilter
 check_PROGRAMS += testRot3Optimization
 

tests/testExtendedKalmanFilter.cpp

@@ -365,8 +365,8 @@ public:
 TEST( ExtendedKalmanFilter, nonlinear ) {
 
   // Create the set of expected output TestValues (generated using Matlab Kalman Filter)
-  Point2 expected_predict[10];
+  Point2 expected_predict[11];
-  Point2 expected_update[10];
+  Point2 expected_update[11];
   expected_predict[1] = Point2(0.81,0.99);
   expected_update[1] =  Point2(0.824926197027,0.29509808);
   expected_predict[2] = Point2(0.680503230541,0.24343413);

tests/testNonlinearFactorGraph.cpp

@@ -75,8 +75,12 @@ TEST( Graph, keys )
 /* ************************************************************************* */
 TEST( Graph, GET_ORDERING)
 {
-  Ordering expected; expected += "x1","l1","x2";
+//  Ordering expected; expected += "x1","l1","x2"; // For starting with x1,x2,l1
+  Ordering expected; expected += "l1","x2","x1"; // For starting with l1,x1,x2
   Graph nlfg = createNonlinearFactorGraph();
+  SymbolicFactorGraph::shared_ptr symbolic;
+  Ordering::shared_ptr ordering;
+  boost::tie(symbolic, ordering) = nlfg.symbolic(createNoisyValues());
   Ordering actual = *nlfg.orderingCOLAMD(createNoisyValues());
   CHECK(assert_equal(expected,actual));
 }