From 617040f503d8f09f73e6c60f13da8e0b75021d52 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sat, 22 Dec 2018 17:09:14 -0500
Subject: [PATCH] count method (better than filter, then size).

---
 gtsam/nonlinear/Values.h             | 11 ++++++++
 gtsam/nonlinear/tests/testValues.cpp |  1 +
 gtsam/slam/dataset.cpp               | 39 ++++++++++++++--------------
 3 files changed, 31 insertions(+), 20 deletions(-)

diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index bf17f1f0d..676d30787 100644
--- a/gtsam/nonlinear/Values.h
+++ b/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>
+    bool 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.
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index bcf01eff5..0dee52570 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -383,6 +383,7 @@ TEST(Values, filter) {
     ++ i;
   }
   EXPECT_LONGS_EQUAL(2, (long)i);
+  EXPECT_LONGS_EQUAL(2, (long)values.count<Pose3>());
 
   // construct a values with the view
   Values actualSubValues2(pose_filtered);
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index ba51864f1..4f52f3c40 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -432,14 +432,14 @@ 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 Values::ConstFiltered<Pose2>::KeyValuePair& key_value: viewPose2) {
+  const auto viewPose2 = estimate.filter<Pose2>();
+  for(const auto& key_value: viewPose2) {
     stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " "
         << key_value.value.y() << " " << key_value.value.theta() << endl;
   }
 
-  Values::ConstFiltered<Pose3> viewPose3 = estimate.filter<Pose3>();
-  for(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value: viewPose3) {
+  const auto viewPose3 = estimate.filter<Pose3>();
+  for(const auto& key_value: viewPose3) {
       Point3 p = key_value.value.translation();
       Rot3 R = key_value.value.rotation();
       stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " "  << p.y() << " " << p.z()
@@ -448,7 +448,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
   }
 
   // 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 +857,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>();
-  if (valuesPoses.size() == dataValues.number_cameras()) { // we only estimated camera poses
+  size_t nrPoses = values.count<Pose3>();
+  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> >();
-    if (valuesCameras.size() == dataValues.number_cameras()) { // we only estimated camera poses and calibration
-      for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
+    size_t nrCameras = values.count<Camera>();
+    if (nrCameras == dataValues.number_cameras()) { // we only estimated camera poses and calibration
+      for (size_t i = 0; i < nrCameras; i++) { // for each camera
         Key cameraKey = i; // symbol('c',i);
-        PinholeCamera<Cal3Bundler> camera =
-            values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
+        Camera camera = values.at<Camera>(cameraKey);
         dataValues.cameras[i] = camera;
       }
     } else {
       cout
           << "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>();
-  if (valuesPoints.size() != dataValues.number_tracks()) {
+  size_t nrPoints = values.count<Point3>(), nrTracks = dataValues.number_tracks();
+  if (nrPoints != nrTracks) {
     cout
         << "writeBALfromValues: different number of points in SfM_dataValues (#points= "
-        << dataValues.number_tracks() << ") and values (#points "
-        << valuesPoints.size() << ")!!" << endl;
+        << nrTracks << ") and values (#points "
+        << 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);