fix extractPoint2/3 and added C++ tests

gtsam/geometry/tests/testUtilities.cpp

@@ -0,0 +1,64 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testUtilities.cpp
+ * @date Aug 19, 2021
+ * @author Varun Agrawal
+ * @brief Tests for the utilities.
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/utilities.h>
+
+using namespace gtsam;
+using gtsam::symbol_shorthand::L;
+using gtsam::symbol_shorthand::R;
+using gtsam::symbol_shorthand::X;
+
+/* ************************************************************************* */
+TEST(Utilities, ExtractPoint2) {
+  Point2 p0(0, 0), p1(1, 0);
+  Values values;
+  values.insert<Point2>(L(0), p0);
+  values.insert<Point2>(L(1), p1);
+  values.insert<Rot3>(R(0), Rot3());
+  values.insert<Pose3>(X(0), Pose3());
+
+  Matrix all_points = utilities::extractPoint2(values);
+  EXPECT_LONGS_EQUAL(2, all_points.rows());
+}
+
+/* ************************************************************************* */
+TEST(Utilities, ExtractPoint3) {
+  Point3 p0(0, 0, 0), p1(1, 0, 0);
+  Values values;
+  values.insert<Point3>(L(0), p0);
+  values.insert<Point3>(L(1), p1);
+  values.insert<Rot3>(R(0), Rot3());
+  values.insert<Pose3>(X(0), Pose3());
+
+  Matrix all_points = utilities::extractPoint3(values);
+  EXPECT_LONGS_EQUAL(2, all_points.rows());
+}
+
+/* ************************************************************************* */
+int main() {
+  srand(time(nullptr));
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/nonlinear/utilities.h

@@ -89,21 +89,41 @@ KeySet createKeySet(std::string s, const Vector& I) {
 
 /// Extract all Point2 values into a single matrix [x y]
 Matrix extractPoint2(const Values& values) {
+  Values::ConstFiltered<gtsam::Point2> points = values.filter<gtsam::Point2>();
+  // Point2 is aliased as a gtsam::Vector in the wrapper
+  Values::ConstFiltered<gtsam::Vector> points2 = values.filter<gtsam::Vector>();
+
+  Matrix result(points.size() + points2.size(), 2);
+
   size_t j = 0;
-  Values::ConstFiltered<Point2> points = values.filter<Point2>();
+  for (const auto& key_value : points) {
-  Matrix result(points.size(), 2);
-  for(const auto& key_value: points)
     result.row(j++) = key_value.value;
+  }
+  for (const auto& key_value : points2) {
+    if (key_value.value.rows() == 2) {
+      result.row(j++) = key_value.value;
+    }
+  }
   return result;
 }
 
 /// Extract all Point3 values into a single matrix [x y z]
 Matrix extractPoint3(const Values& values) {
-  Values::ConstFiltered<Point3> points = values.filter<Point3>();
+  Values::ConstFiltered<gtsam::Point3> points = values.filter<gtsam::Point3>();
-  Matrix result(points.size(), 3);
+  // Point3 is aliased as a gtsam::Vector in the wrapper
+  Values::ConstFiltered<gtsam::Vector> points2 = values.filter<gtsam::Vector>();
+
+  Matrix result(points.size() + points2.size(), 3);
+
   size_t j = 0;
-  for(const auto& key_value: points)
+  for (const auto& key_value : points) {
     result.row(j++) = key_value.value;
+  }
+  for (const auto& key_value : points2) {
+    if (key_value.value.rows() == 3) {
+      result.row(j++) = key_value.value;
+    }
+  }
   return result;
 }