From d074dbedf50d99045ed425d1c59edf90e45d224a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 20 Aug 2021 00:55:10 -0400
Subject: [PATCH] updated docs

---
 gtsam/nonlinear/utilities.h | 56 +++++++++++++++++++++++++++++--------
 1 file changed, 44 insertions(+), 12 deletions(-)

diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h
index 5a7b4f473..fdc1da2c4 100644
--- a/gtsam/nonlinear/utilities.h
+++ b/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
  */
@@ -164,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()));
+    }
   }
 }
 
@@ -185,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");
@@ -208,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()) {