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);
-void perturbPoint3(gtsam::Values& values, double sigma, int seed);
+                  int seed = 42u);
+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,
-                             Vector J, Matrix Z,
-                             const gtsam::noiseModel::Base* model,
-                             const gtsam::Cal3_S2* K);
-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);
+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 = gtsam::Pose3());
 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>();
-  Matrix result(points.size(), 2);
-  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() == 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>();
-  Matrix result(points.size(), 3);
+  Values::ConstFiltered<gtsam::Point3> points = values.filter<gtsam::Point3>();
+  // 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,
-      sigma);
+  noiseModel::Isotropic::shared_ptr model =
+      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()));
+  for (const auto& key_value : values.filter<Point2>()) {
+    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,
-      sigma);
+  noiseModel::Isotropic::shared_ptr model =
+      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()));
+  for (const auto& key_value : values.filter<Point3>()) {
+    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()) {