Merged in fix/GenericValue-assignment (pull request #345)

close Issue #412
Attempt to fix GenericValue assignment operator for windows

gtsam/base/GenericValue.h

@@ -164,11 +164,11 @@ public:
 
   protected:
 
-    // implicit assignment operator for (const GenericValue& rhs) works fine here
     /// Assignment operator, protected because only the Value or DERIVED
     /// assignment operators should be used.
     GenericValue<T>& operator=(const GenericValue<T>& rhs) {
-      // Nothing to do, do not call base class assignment operator
+      Value::operator=(static_cast<Value const&>(rhs));
+      value_ = rhs.value_;
       return *this;
     }
 

gtsam/nonlinear/Values.h

@@ -385,6 +385,17 @@ namespace gtsam {
     ConstFiltered<ValueType>
     filter(const boost::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
 
+    // Count values of given type \c ValueType
+    template<class ValueType>
+    size_t count() const {
+      size_t i = 0;
+      for (const auto& key_value : *this) {
+        if (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value))
+          ++i;
+      }
+      return i;
+    }
+
   private:
     // Filters based on ValueType (if not Value) and also based on the user-
     // supplied \c filter function.

gtsam/nonlinear/tests/testValues.cpp

@@ -383,6 +383,8 @@ TEST(Values, filter) {
     ++ i;
   }
   EXPECT_LONGS_EQUAL(2, (long)i);
+  EXPECT_LONGS_EQUAL(2, (long)values.count<Pose3>());
+  EXPECT_LONGS_EQUAL(2, (long)values.count<Pose2>());
 
   // construct a values with the view
   Values actualSubValues2(pose_filtered);

gtsam/slam/dataset.cpp

@@ -432,23 +432,27 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
   fstream stream(filename.c_str(), fstream::out);
 
   // save 2D & 3D poses
-  Values::ConstFiltered<Pose2> viewPose2 = estimate.filter<Pose2>();
+  for (const auto& key_value : estimate) {
-  for(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value: viewPose2) {
+    auto p = dynamic_cast<const GenericValue<Pose2>*>(&key_value.value);
-    stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " "
+    if (!p) continue;
-        << key_value.value.y() << " " << key_value.value.theta() << endl;
+    const Pose2& pose = p->value();
+    stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
+        << pose.y() << " " << pose.theta() << endl;
   }
 
-  Values::ConstFiltered<Pose3> viewPose3 = estimate.filter<Pose3>();
+  for(const auto& key_value: estimate) {
-  for(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value: viewPose3) {
+      auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
-      Point3 p = key_value.value.translation();
+      if (!p) continue;
-      Rot3 R = key_value.value.rotation();
+      const Pose3& pose = p->value();
-      stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " "  << p.y() << " " << p.z()
+      Point3 t = pose.translation();
+      Rot3 R = pose.rotation();
+      stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " "  << t.y() << " " << t.z()
         << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z()
         << " " << R.toQuaternion().w() << endl;
   }
 
   // save edges (2D or 3D)
-  for(boost::shared_ptr<NonlinearFactor> factor_: graph) {
+  for(const auto& factor_: graph) {
     boost::shared_ptr<BetweenFactor<Pose2> > factor =
         boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
     if (factor){
@@ -857,48 +861,47 @@ bool writeBAL(const string& filename, SfM_data &data) {
 
 bool writeBALfromValues(const string& filename, const SfM_data &data,
     Values& values) {
-
+  using Camera = PinholeCamera<Cal3Bundler>;
   SfM_data dataValues = data;
 
   // Store poses or cameras in SfM_data
-  Values valuesPoses = values.filter<Pose3>();
+  size_t nrPoses = values.count<Pose3>();
-  if (valuesPoses.size() == dataValues.number_cameras()) { // we only estimated camera poses
+  if (nrPoses == dataValues.number_cameras()) { // we only estimated camera poses
     for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
       Key poseKey = symbol('x', i);
       Pose3 pose = values.at<Pose3>(poseKey);
       Cal3Bundler K = dataValues.cameras[i].calibration();
-      PinholeCamera<Cal3Bundler> camera(pose, K);
+      Camera camera(pose, K);
       dataValues.cameras[i] = camera;
     }
   } else {
-    Values valuesCameras = values.filter<PinholeCamera<Cal3Bundler> >();
+    size_t nrCameras = values.count<Camera>();
-    if (valuesCameras.size() == dataValues.number_cameras()) { // we only estimated camera poses and calibration
+    if (nrCameras == dataValues.number_cameras()) { // we only estimated camera poses and calibration
-      for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
+      for (size_t i = 0; i < nrCameras; i++) { // for each camera
         Key cameraKey = i; // symbol('c',i);
-        PinholeCamera<Cal3Bundler> camera =
+        Camera camera = values.at<Camera>(cameraKey);
-            values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
         dataValues.cameras[i] = camera;
       }
     } else {
       cout
-          << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= "
+          << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras "
           << dataValues.number_cameras() << ") and values (#cameras "
-          << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!"
+          << nrPoses << ", #poses " << nrCameras << ")!!"
           << endl;
       return false;
     }
   }
 
   // Store 3D points in SfM_data
-  Values valuesPoints = values.filter<Point3>();
+  size_t nrPoints = values.count<Point3>(), nrTracks = dataValues.number_tracks();
-  if (valuesPoints.size() != dataValues.number_tracks()) {
+  if (nrPoints != nrTracks) {
     cout
         << "writeBALfromValues: different number of points in SfM_dataValues (#points= "
-        << dataValues.number_tracks() << ") and values (#points "
+        << nrTracks << ") and values (#points "
-        << valuesPoints.size() << ")!!" << endl;
+        << nrPoints << ")!!" << endl;
   }
 
-  for (size_t j = 0; j < dataValues.number_tracks(); j++) { // for each point
+  for (size_t j = 0; j < nrTracks; j++) { // for each point
     Key pointKey = P(j);
     if (values.exists(pointKey)) {
       Point3 point = values.at<Point3>(pointKey);

tests/testVisualISAM2.cpp

@@ -0,0 +1,127 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testVisualISAM2.cpp
+ * @brief   Test convergence of visualSLAM example.
+ * @author  Duy-Nguyen Ta
+ * @author  Frank Dellaert
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <examples/SFMdata.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/ProjectionFactor.h>
+
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST(testVisualISAM2, all)
+{
+    Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
+
+    auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
+
+    // Create ground truth data
+    vector<Point3> points = createPoints();
+    vector<Pose3> poses = createPoses();
+
+    // Set the parameters
+    ISAM2Params parameters;
+    parameters.relinearizeThreshold = 0.01;
+    parameters.relinearizeSkip = 1;
+    ISAM2 isam(parameters);
+
+    // Create a Factor Graph and Values to hold the new data
+    NonlinearFactorGraph graph;
+    Values initialEstimate;
+
+    // Loop over the poses, adding the observations to iSAM incrementally
+    for (size_t i = 0; i < poses.size(); ++i)
+    {
+        // Add factors for each landmark observation
+        for (size_t j = 0; j < points.size(); ++j)
+        {
+            SimpleCamera camera(poses[i], *K);
+            Point2 measurement = camera.project(points[j]);
+            graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>(
+                measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
+        }
+
+        // Add an initial guess for the current pose
+        // Intentionally initialize the variables off from the ground truth
+        static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25),
+                                Point3(0.05, -0.10, 0.20));
+        initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose);
+
+        // Treat first iteration as special case
+        if (i == 0)
+        {
+            // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw
+            static auto kPosePrior = noiseModel::Diagonal::Sigmas(
+                (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))
+                    .finished());
+            graph.emplace_shared<PriorFactor<Pose3>>(Symbol('x', 0), poses[0],
+                                                     kPosePrior);
+
+            // Add a prior on landmark l0
+            static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
+            graph.emplace_shared<PriorFactor<Point3>>(Symbol('l', 0), points[0],
+                                                      kPointPrior);
+
+            // Add initial guesses to all observed landmarks
+            // Intentionally initialize the variables off from the ground truth
+            static Point3 kDeltaPoint(-0.25, 0.20, 0.15);
+            for (size_t j = 0; j < points.size(); ++j)
+                initialEstimate.insert<Point3>(Symbol('l', j), points[j] + kDeltaPoint);
+        }
+        else
+        {
+            // Update iSAM with the new factors
+            isam.update(graph, initialEstimate);
+
+            // Do an extra update to converge withing these 8 iterations
+            isam.update();
+
+            // Optimize
+            Values currentEstimate = isam.calculateEstimate();
+
+            // reset for next iteration
+            graph.resize(0);
+            initialEstimate.clear();
+        }
+    } // for loop
+
+    auto result = isam.calculateEstimate();
+    EXPECT_LONGS_EQUAL(16, result.size());
+    for (size_t j = 0; j < points.size(); ++j)
+    {
+        Symbol key('l', j);
+        EXPECT(assert_equal(points[j], result.at<Point3>(key), 0.01));
+    }
+}
+
+/* ************************************************************************* */
+int main()
+{
+    TestResult tr;
+    return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */