Merge pull request #851 from borglab/fix/extract-point2

Fix utilities and add 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/gtsam.i

@@ -165,22 +165,17 @@ gtsam::Values allPose2s(gtsam::Values& values);
 Matrix extractPose2(const gtsam::Values& values);
 gtsam::Values allPose3s(gtsam::Values& values);
 Matrix extractPose3(const gtsam::Values& values);
-void perturbPoint2(gtsam::Values& values, double sigma, int seed);
+void perturbPoint2(gtsam::Values& values, double sigma, int seed = 42u);
 void perturbPose2(gtsam::Values& values, double sigmaT, double sigmaR,
-                  int seed);
+                  int seed = 42u);
-void perturbPoint3(gtsam::Values& values, double sigma, int seed);
+void perturbPoint3(gtsam::Values& values, double sigma, int seed = 42u);
 void insertBackprojections(gtsam::Values& values,
                            const gtsam::PinholeCamera<gtsam::Cal3_S2>& c,
                            Vector J, Matrix Z, double depth);
-void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i,
+void insertProjectionFactors(
-                             Vector J, Matrix Z,
+    gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z,
-                             const gtsam::noiseModel::Base* model,
+    const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K,
-                             const gtsam::Cal3_S2* K);
+    const gtsam::Pose3& body_P_sensor = gtsam::Pose3());
-void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i,
-                             Vector J, Matrix Z,
-                             const gtsam::noiseModel::Base* model,
-                             const gtsam::Cal3_S2* K,
-                             const gtsam::Pose3& body_P_sensor);
 Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph,
                           const gtsam::Values& values);
 gtsam::Values localToWorld(const gtsam::Values& local,

gtsam/nonlinear/utilities.h

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    matlab.h
+ * @file    utilities.h
  * @brief   Contains *generic* global functions designed particularly for the matlab interface
  * @author  Stephen Williams
  */
@@ -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;
 }
 
@@ -144,11 +164,18 @@ Matrix extractPose3(const Values& values) {
 
 /// Perturb all Point2 values using normally distributed noise
 void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) {
-  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,
+  noiseModel::Isotropic::shared_ptr model =
-      sigma);
+      noiseModel::Isotropic::Sigma(2, sigma);
   Sampler sampler(model, seed);
   for (const auto& key_value : values.filter<Point2>()) {
-    values.update<Point2>(key_value.key, key_value.value + Point2(sampler.sample()));
+    values.update<Point2>(key_value.key,
+                          key_value.value + Point2(sampler.sample()));
+  }
+  for (const auto& key_value : values.filter<gtsam::Vector>()) {
+    if (key_value.value.rows() == 2) {
+      values.update<gtsam::Vector>(key_value.key,
+                                   key_value.value + Point2(sampler.sample()));
+    }
   }
 }
 
@@ -165,19 +192,34 @@ void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed =
 
 /// Perturb all Point3 values using normally distributed noise
 void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) {
-  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,
+  noiseModel::Isotropic::shared_ptr model =
-      sigma);
+      noiseModel::Isotropic::Sigma(3, sigma);
   Sampler sampler(model, seed);
   for (const auto& key_value : values.filter<Point3>()) {
-    values.update<Point3>(key_value.key, key_value.value + Point3(sampler.sample()));
+    values.update<Point3>(key_value.key,
+                          key_value.value + Point3(sampler.sample()));
+  }
+  for (const auto& key_value : values.filter<gtsam::Vector>()) {
+    if (key_value.value.rows() == 3) {
+      values.update<gtsam::Vector>(key_value.key,
+                                   key_value.value + Point3(sampler.sample()));
+    }
   }
 }
 
-/// Insert a number of initial point values by backprojecting
+/**
+ * @brief Insert a number of initial point values by backprojecting
+ * 
+ * @param values The values dict to insert the backprojections to.
+ * @param camera The camera model.
+ * @param J Vector of key indices.
+ * @param Z 2*J matrix of pixel values.
+ * @param depth Initial depth value.
+ */
 void insertBackprojections(Values& values, const PinholeCamera<Cal3_S2>& camera,
     const Vector& J, const Matrix& Z, double depth) {
   if (Z.rows() != 2)
-    throw std::invalid_argument("insertBackProjections: Z must be 2*K");
+    throw std::invalid_argument("insertBackProjections: Z must be 2*J");
   if (Z.cols() != J.size())
     throw std::invalid_argument(
         "insertBackProjections: J and Z must have same number of entries");
@@ -188,7 +230,17 @@ void insertBackprojections(Values& values, const PinholeCamera<Cal3_S2>& camera,
   }
 }
 
-/// Insert multiple projection factors for a single pose key
+/**
+ * @brief Insert multiple projection factors for a single pose key
+ * 
+ * @param graph The nonlinear factor graph to add the factors to.
+ * @param i Camera key.
+ * @param J Vector of key indices.
+ * @param Z 2*J matrix of pixel values.
+ * @param model Factor noise model.
+ * @param K Calibration matrix.
+ * @param body_P_sensor Pose of the camera sensor in the body frame.
+ */
 void insertProjectionFactors(NonlinearFactorGraph& graph, Key i,
     const Vector& J, const Matrix& Z, const SharedNoiseModel& model,
     const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) {

python/gtsam/tests/test_Utilities.py

@@ -0,0 +1,196 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Utilities unit tests.
+Author: Varun Agrawal
+"""
+# pylint: disable=invalid-name, E1101, E0611
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestUtilites(GtsamTestCase):
+    """Test various GTSAM utilities."""
+    def test_createKeyList(self):
+        """Test createKeyList."""
+        I = [0, 1, 2]
+        kl = gtsam.utilities.createKeyList(I)
+        self.assertEqual(kl.size(), 3)
+
+        kl = gtsam.utilities.createKeyList("s", I)
+        self.assertEqual(kl.size(), 3)
+
+    def test_createKeyVector(self):
+        """Test createKeyVector."""
+        I = [0, 1, 2]
+        kl = gtsam.utilities.createKeyVector(I)
+        self.assertEqual(len(kl), 3)
+
+        kl = gtsam.utilities.createKeyVector("s", I)
+        self.assertEqual(len(kl), 3)
+
+    def test_createKeySet(self):
+        """Test createKeySet."""
+        I = [0, 1, 2]
+        kl = gtsam.utilities.createKeySet(I)
+        self.assertEqual(kl.size(), 3)
+
+        kl = gtsam.utilities.createKeySet("s", I)
+        self.assertEqual(kl.size(), 3)
+
+    def test_extractPoint2(self):
+        """Test extractPoint2."""
+        initial = gtsam.Values()
+        point2 = gtsam.Point2(0.0, 0.1)
+        initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1))
+        initial.insert(2, point2)
+        np.testing.assert_equal(gtsam.utilities.extractPoint2(initial),
+                                point2.reshape(-1, 2))
+
+    def test_extractPoint3(self):
+        """Test extractPoint3."""
+        initial = gtsam.Values()
+        point3 = gtsam.Point3(0.0, 0.1, 0.0)
+        initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1))
+        initial.insert(2, point3)
+        np.testing.assert_equal(gtsam.utilities.extractPoint3(initial),
+                                point3.reshape(-1, 3))
+
+    def test_allPose2s(self):
+        """Test allPose2s."""
+        initial = gtsam.Values()
+        initial.insert(0, gtsam.Pose2())
+        initial.insert(1, gtsam.Pose2(1, 1, 1))
+        initial.insert(2, gtsam.Point2(1, 1))
+        initial.insert(3, gtsam.Point3(1, 2, 3))
+        result = gtsam.utilities.allPose2s(initial)
+        self.assertEqual(result.size(), 2)
+
+    def test_extractPose2(self):
+        """Test extractPose2."""
+        initial = gtsam.Values()
+        pose2 = np.asarray((0.0, 0.1, 0.1))
+
+        initial.insert(1, gtsam.Pose2(*pose2))
+        initial.insert(2, gtsam.Point3(0.0, 0.1, 0.0))
+        np.testing.assert_allclose(gtsam.utilities.extractPose2(initial),
+                                   pose2.reshape(-1, 3))
+
+    def test_allPose3s(self):
+        """Test allPose3s."""
+        initial = gtsam.Values()
+        initial.insert(0, gtsam.Pose3())
+        initial.insert(2, gtsam.Point2(1, 1))
+        initial.insert(1, gtsam.Pose3())
+        initial.insert(3, gtsam.Point3(1, 2, 3))
+        result = gtsam.utilities.allPose3s(initial)
+        self.assertEqual(result.size(), 2)
+
+    def test_extractPose3(self):
+        """Test extractPose3."""
+        initial = gtsam.Values()
+        pose3 = np.asarray([1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0.])
+        initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1))
+        initial.insert(2, gtsam.Pose3())
+        np.testing.assert_allclose(gtsam.utilities.extractPose3(initial),
+                                   pose3.reshape(-1, 12))
+
+    def test_perturbPoint2(self):
+        """Test perturbPoint2."""
+        values = gtsam.Values()
+        values.insert(0, gtsam.Pose3())
+        values.insert(1, gtsam.Point2(1, 1))
+        gtsam.utilities.perturbPoint2(values, 1.0)
+        self.assertTrue(
+            not np.allclose(values.atPoint2(1), gtsam.Point2(1, 1)))
+
+    def test_perturbPose2(self):
+        """Test perturbPose2."""
+        values = gtsam.Values()
+        values.insert(0, gtsam.Pose2())
+        values.insert(1, gtsam.Point2(1, 1))
+        gtsam.utilities.perturbPose2(values, 1, 1)
+        self.assertTrue(values.atPose2(0) != gtsam.Pose2())
+
+    def test_perturbPoint3(self):
+        """Test perturbPoint3."""
+        values = gtsam.Values()
+        point3 = gtsam.Point3(0, 0, 0)
+        values.insert(0, gtsam.Pose2())
+        values.insert(1, point3)
+        gtsam.utilities.perturbPoint3(values, 1)
+        self.assertTrue(not np.allclose(values.atPoint3(1), point3))
+
+    def test_insertBackprojections(self):
+        """Test insertBackprojections."""
+        values = gtsam.Values()
+        cam = gtsam.PinholeCameraCal3_S2()
+        gtsam.utilities.insertBackprojections(
+            values, cam, [0, 1, 2], np.asarray([[20, 30, 40], [20, 30, 40]]),
+            10)
+        np.testing.assert_allclose(values.atPoint3(0),
+                                   gtsam.Point3(200, 200, 10))
+
+    def test_insertProjectionFactors(self):
+        """Test insertProjectionFactors."""
+        graph = gtsam.NonlinearFactorGraph()
+        gtsam.utilities.insertProjectionFactors(
+            graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]),
+            gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2())
+        self.assertEqual(graph.size(), 2)
+
+        graph = gtsam.NonlinearFactorGraph()
+        gtsam.utilities.insertProjectionFactors(
+            graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]),
+            gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2(),
+            gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0)))
+        self.assertEqual(graph.size(), 2)
+
+    def test_reprojectionErrors(self):
+        """Test reprojectionErrors."""
+        pixels = np.asarray([[20, 30], [20, 30]])
+        I = [1, 2]
+        K = gtsam.Cal3_S2()
+        graph = gtsam.NonlinearFactorGraph()
+        gtsam.utilities.insertProjectionFactors(
+            graph, 0, I, pixels, gtsam.noiseModel.Isotropic.Sigma(2, 0.1), K)
+        values = gtsam.Values()
+        values.insert(0, gtsam.Pose3())
+        cam = gtsam.PinholeCameraCal3_S2(gtsam.Pose3(), K)
+        gtsam.utilities.insertBackprojections(values, cam, I, pixels, 10)
+        errors = gtsam.utilities.reprojectionErrors(graph, values)
+        np.testing.assert_allclose(errors, np.zeros((2, 2)))
+
+    def test_localToWorld(self):
+        """Test localToWorld."""
+        local = gtsam.Values()
+        local.insert(0, gtsam.Point2(10, 10))
+        local.insert(1, gtsam.Pose2(6, 11, 0.0))
+        base = gtsam.Pose2(1, 0, 0)
+        world = gtsam.utilities.localToWorld(local, base)
+
+        expected_point2 = gtsam.Point2(11, 10)
+        expected_pose2 = gtsam.Pose2(7, 11, 0)
+        np.testing.assert_allclose(world.atPoint2(0), expected_point2)
+        np.testing.assert_allclose(
+            world.atPose2(1).matrix(), expected_pose2.matrix())
+
+        user_keys = [1]
+        world = gtsam.utilities.localToWorld(local, base, user_keys)
+        np.testing.assert_allclose(
+            world.atPose2(1).matrix(), expected_pose2.matrix())
+
+        # Raise error since 0 is not in user_keys
+        self.assertRaises(RuntimeError, world.atPoint2, 0)
+
+
+if __name__ == "__main__":
+    unittest.main()