Document all methods / classes

Author: contagon

config/25.10.08_short_verify.yaml

@@ -1,6 +1,6 @@
 pipelines:
   - pipeline: form
-    name: keypoint_class_final
+    name: 2_docs_fixed
 
 datasets:
   # ncd20

form/feature/extraction.hpp

@@ -1,3 +1,28 @@
+// MIT License
+
+// Copyright (c) 2025 Easton Potokar, Taylor Pool, and Michael Kaess
+
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+/// This extraction code is largely based on Dan McGann's LOAM implementation
+/// (https://github.com/DanMcGann/loam/), with some modifications to remove edge
+/// features extraction and add in normal estimation.
 #pragma once
 
 #include "form/feature/features.hpp"
@@ -28,78 +53,105 @@ template <typename T> struct Curvature {
   }
 };
 
+/// @brief Class for extracting planar and point features from a LiDAR scan
 class FeatureExtractor {
 public:
   struct Params {
-    // Parameters for keypoint extraction
+    // KEYPOINT EXTRACTION PARAMETERS
+    /// @brief Number of neighboring points to use for curvature computation and for
+    /// normal estimation
     size_t neighbor_points = 5;
+    /// @brief Number of sectors to divide the scan into for feature extraction
     size_t num_sectors = 6;
+    /// @brief Threshold for planar feature extraction
     double planar_threshold = 1.0;
+    /// @brief Number of planar features to extract per sector
     size_t planar_feats_per_sector = 50;
+    /// @brief Number of point features to extract per sector
     size_t point_feats_per_sector = 3;
 
-    // Parameters for normal estimation
+    // NORMAL ESTIMATION PARAMETERS
+    /// @brief Radius for neighborhood search
     double radius = 1.0;
+    /// @brief Minimum number of points required for normal estimation
     size_t min_points = 5;
 
-    // Based on LiDAR info
+    // MISC LiDAR PARAMETERS
+    /// @brief Minimum range for a point to be considered valid
     double min_norm_squared = 1.0;
+    /// @brief Maximum range for a point to be considered valid
     double max_norm_squared = 100.0 * 100.0;
+    /// @brief Number of columns in the LiDAR scan
     int num_columns = 1024;
+    /// @brief Number of rows aka scanlines in the LiDAR scan
     int num_rows = 64;
   };
 
   Params params;
 
-  FeatureExtractor(const Params &params, size_t num_threads) : params(params) {
+  /// @brief Explicit parameterized constructor
+  FeatureExtractor(const Params &params, size_t num_threads = 0) : params(params) {
     static const auto tbb_control_settings = set_num_threads(num_threads);
   }
 
   // ------------------------- Entrypoint ------------------------- //
+  /// @brief Extract planar and point features from a LiDAR scan
   template <typename Point>
   [[nodiscard]] std::tuple<std::vector<PlanarFeat>, std::vector<PointFeat>>
   extract(const std::vector<Point> &scan, size_t scan_idx) const;
 
 private:
   // ------------------------- Validators ------------------------- //
+  /// @brief Compute a mask of valid points based on range and NaN checks, with
+  /// neighbors also marked invalid for planar feature extraction
   template <typename Point>
   std::vector<bool> compute_valid_points(const std::vector<Point> &scan) const;
 
+  /// @brief Compute a mask of valid points based on range and NaN checks, without
+  /// neighbors marked invalid for point feature extraction
   template <typename Point>
   std::vector<bool> compute_point_valid_points(const std::vector<Point> &scan) const;
 
   // ------------------------- Computers ------------------------- //
+  /// @brief Compute the curvature for each point in the scan
   template <typename Point>
   std::vector<Curvature<typename Point::Scalar>>
   compute_curvature(const std::vector<Point> &scan, const std::vector<bool> &mask,
                     size_t scan_idx) const noexcept;
 
+  /// @brief Compute the normal for a point given its index, the scan, and a validity
+  /// mask
   template <typename Point>
   std::optional<Eigen::Matrix<typename Point::Scalar, 3, 1>>
   compute_normal(const size_t &idx, const std::vector<Point> &scan,
                  const std::vector<bool> &valid_mask) const noexcept;
 
   // ------------------------- Extractors ------------------------- //
-
+  /// @brief Extract planar features from a sector of the scan based on curvature
   template <typename T>
   void extract_planar(const size_t &sector_start_point,
                       const size_t &sector_end_point,
                       const std::vector<Curvature<T>> &curvature,
                       std::vector<size_t> &out_features,
                       std::vector<bool> &valid_mask) const noexcept;
 
+  /// @brief Extract point features from a sector of the scan
   inline void extract_point(const size_t &sector_start_point,
                             const size_t &sector_end_point,
                             std::vector<size_t> &out_features,
                             std::vector<bool> &valid_mask) const noexcept;
 
   // ------------------------- Helpers ------------------------- //
+  /// @brief Find the index of the closest valid point to a given point within a
+  /// range (range is usually a scanline)
   template <typename Point>
   std::optional<size_t>
   find_closest(const Point &point, const size_t &start, const size_t &end,
                const std::vector<Point> &scan,
                const std::vector<bool> &valid_mask) const noexcept;
 
+  /// @brief Find the neighboring points of a given point index in the scan, only
+  /// including points within neighbor_points and radius
   template <typename Point>
   void find_neighbors(const size_t &idx, const std::vector<Point> &scan,
                       std::vector<Point> &out) const noexcept;

form/feature/extraction.tpp

@@ -1,3 +1,24 @@
+// MIT License
+
+// Copyright (c) 2025 Easton Potokar, Taylor Pool, and Michael Kaess
+
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
 #pragma once
 
 #include "form/feature/extraction.hpp"

form/feature/factor.cpp

@@ -1,3 +1,24 @@
+// MIT License
+
+// Copyright (c) 2025 Easton Potokar, Taylor Pool, and Michael Kaess
+
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
 #include "form/feature/factor.hpp"
 #include "form/optimization/gtsam.hpp"
 #include <Eigen/src/Core/Matrix.h>
@@ -119,14 +140,14 @@ FeatureFactor::FeatureFactor(
           FastIsotropic::Sigma(sigma, std::get<0>(constraints)->num_residuals() +
                                           std::get<1>(constraints)->num_residuals()),
           i, j),
-      plane_plane(std::get<0>(constraints)), point_point(std::get<1>(constraints)) {}
+      plane_point(std::get<0>(constraints)), point_point(std::get<1>(constraints)) {}
 
 [[nodiscard]] gtsam::Vector
 FeatureFactor::evaluateError(const gtsam::Pose3 &Ti, const gtsam::Pose3 &Tj,
                              boost::optional<gtsam::Matrix &> H1,
                              boost::optional<gtsam::Matrix &> H2) const noexcept {
 
-  size_t size = plane_plane->num_residuals() + point_point->num_residuals();
+  size_t size = plane_point->num_residuals() + point_point->num_residuals();
   gtsam::Vector residual(size);
 
   if (H1) {
@@ -137,11 +158,11 @@ FeatureFactor::evaluateError(const gtsam::Pose3 &Ti, const gtsam::Pose3 &Tj,
   }
 
   // Plane-Plane constraints
-  size_t start = 0, end = plane_plane->num_residuals();
-  if (plane_plane->num_residuals() > 0) {
+  size_t start = 0, end = plane_point->num_residuals();
+  if (plane_point->num_residuals() > 0) {
     Eigen::MatrixXd H1_temp, H2_temp;
     residual.segment(start, end - start) =
-        plane_plane->evaluateError(Ti, Tj, H1 ? &H1_temp : 0, H2 ? &H2_temp : 0);
+        plane_point->evaluateError(Ti, Tj, H1 ? &H1_temp : 0, H2 ? &H2_temp : 0);
     if (H1) {
       H1->block(start, 0, end - start, 6) = H1_temp;
     }

form/feature/factor.hpp

@@ -1,3 +1,24 @@
+// MIT License
+
+// Copyright (c) 2025 Easton Potokar, Taylor Pool, and Michael Kaess
+
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
 #pragma once
 
 #include <gtsam/geometry/Point3.h>
@@ -17,82 +38,131 @@ static void check(const gtsam::SharedNoiseModel &noiseModel, size_t m) {
 
 using OptionalJacobian = gtsam::OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>;
 
+/// @brief Structure that holds plane-point correspondences and computes their error
+///
+/// Points and normals are stored in a std::vector that is then mapped to Eigen
+/// matrices for efficient computation.
 struct PlanePoint {
+  /// @brief Shared pointer type
   typedef std::shared_ptr<PlanePoint> Ptr;
 
+  /// @brief Vectors to hold point and normal data
   std::vector<double> p_i;
   std::vector<double> n_i;
   std::vector<double> p_j;
 
   PlanePoint() = default;
 
+  /// @brief Add a new plane-point correspondence
   void push_back(const PlanarFeat &p_i_, const PlanarFeat &p_j_) {
     p_i.insert(p_i.end(), {p_i_.x, p_i_.y, p_i_.z});
     n_i.insert(n_i.end(), {p_i_.nx, p_i_.ny, p_i_.nz});
     p_j.insert(p_j.end(), {p_j_.x, p_j_.y, p_j_.z});
   }
 
+  /// @brief Clear all stored correspondences
   void clear() noexcept {
     p_i.clear();
     n_i.clear();
     p_j.clear();
   }
 
+  /// @brief Get the number of residuals (one per correspondence)
   size_t num_residuals() const noexcept { return p_i.size() / 3; }
+
+  /// @brief Get the number of constraints / correspondences
   size_t num_constraints() const noexcept { return p_i.size() / 3; }
 
+  /// @brief Evaluate the residual given two poses
+  ///
+  /// @param Ti The pose of the first frame
+  /// @param Tj The pose of the second frame
+  /// @param residual_D_Ti Optional Jacobian of the residual w.r.t. Ti
+  /// @param residual_D_Tj Optional Jacobian of the residual w.r.t. Tj
+  /// @return The residual vector
   [[nodiscard]] gtsam::Vector
   evaluateError(const gtsam::Pose3 &Ti, const gtsam::Pose3 &Tj,
                 OptionalJacobian residual_D_Ti = boost::none,
                 OptionalJacobian residual_D_Tj = boost::none) const noexcept;
 };
 
+/// @brief Structure that holds point-point correspondences and computes their error
+///
+/// Points are stored in a std::vector that is then mapped to Eigen matrices for
+/// efficient computation.
 struct PointPoint {
+  /// @brief Shared pointer type
   typedef std::shared_ptr<PointPoint> Ptr;
 
+  /// @brief Vectors to hold point data
   std::vector<double> p_i;
   std::vector<double> p_j;
 
   PointPoint() = default;
 
-  size_t num_residuals() const noexcept { return p_i.size(); }
-  size_t num_constraints() const noexcept { return p_i.size() / 3; }
-
+  /// @brief Add a new point-point correspondence
   void push_back(const PointFeat &p_i_, const PointFeat &p_j_) {
     p_i.insert(p_i.end(), {p_i_.x, p_i_.y, p_i_.z});
     p_j.insert(p_j.end(), {p_j_.x, p_j_.y, p_j_.z});
   }
 
+  /// @brief Clear all stored correspondences
   void clear() noexcept {
     p_i.clear();
     p_j.clear();
   }
 
+  /// @brief Get the number of residuals (three per correspondence)
+  size_t num_residuals() const noexcept { return p_i.size(); }
+
+  /// @brief Get the number of constraints / correspondences
+  size_t num_constraints() const noexcept { return p_i.size() / 3; }
+
+  /// @brief Evaluate the residual given two poses
+  ///
+  /// @param Ti The pose of the first frame
+  /// @param Tj The pose of the second frame
+  /// @param residual_D_Ti Optional Jacobian of the residual w.r.t. Ti
+  /// @param residual_D_Tj Optional Jacobian of the residual w.r.t. Tj
+  /// @return The residual vector
   [[nodiscard]] gtsam::Vector
   evaluateError(const gtsam::Pose3 &Ti, const gtsam::Pose3 &Tj,
                 OptionalJacobian residual_D_Ti = boost::none,
                 OptionalJacobian residual_D_Tj = boost::none) const noexcept;
 };
 
+/// @brief A wrapper factor that holds both plane-point and point-point constraints
 class FeatureFactor : public DenseFactor {
 public:
-  PlanePoint::Ptr plane_plane;
+  /// @brief Plane-point constraints
+  PlanePoint::Ptr plane_point;
+
+  /// @brief Point-point constraints
   PointPoint::Ptr point_point;
 
 public:
+  /// @brief Constructor
+  ///
+  /// @param i Key for the first pose
+  /// @param j Key for the second pose
+  /// @param constraint A tuple containing plane-point and point-point constraints
+  /// @param sigma The isotropic noise standard deviation
   FeatureFactor(const gtsam::Key i, const gtsam::Key j,
                 const std::tuple<PlanePoint::Ptr, PointPoint::Ptr> &constraint,
                 double sigma) noexcept;
 
+  /// @brief Evaluate the residual given two poses
+  ///
+  /// @param Ti The pose of the first frame
+  /// @param Tj The pose of the second frame
+  /// @param residual_D_Ti Optional Jacobian of the residual w.r.t. Ti
+  /// @param residual_D_Tj Optional Jacobian of the residual w.r.t. Tj
+  /// @return The residual vector
   [[nodiscard]] gtsam::Vector
   evaluateError(const gtsam::Pose3 &Ti, const gtsam::Pose3 &Tj,
                 boost::optional<gtsam::Matrix &> residual_D_Ti = boost::none,
                 boost::optional<gtsam::Matrix &> residual_D_Tj =
                     boost::none) const noexcept override;
-
-  [[nodiscard]] gtsam::Key getKey_i() const noexcept { return keys_[0]; }
-
-  [[nodiscard]] gtsam::Key getKey_j() const noexcept { return keys_[1]; }
 };
 
 } // namespace form