Clean up match class a smidge

Author: contagon

config/25.10.08_short_verify.yaml

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

form/feature/factor.cpp

@@ -21,10 +21,6 @@
 // SOFTWARE.
 #include "form/feature/factor.hpp"
 #include "form/optimization/gtsam.hpp"
-#include <Eigen/src/Core/Matrix.h>
-#include <Eigen/src/Core/util/Constants.h>
-#include <gtsam/base/Matrix.h>
-#include <gtsam/base/Vector.h>
 #include <gtsam/linear/HessianFactor.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/NoiseModel.h>

form/feature/factor.hpp

@@ -21,9 +21,7 @@
 // SOFTWARE.
 #pragma once
 
-#include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/nonlinear/NonlinearFactor.h>
 #include <vector>
 
 #include "form/feature/features.hpp"

form/form.cpp

@@ -66,15 +66,16 @@ Estimator::register_scan(const std::vector<Eigen::Vector3f> &scan) noexcept {
 
   // ICP loop
   gtsam::Values new_values;
+  auto estimates = [&](size_t i) { return m_constraints.get_pose(i); };
   for (size_t idx = 0; idx < m_params.matcher.max_num_rematches; ++idx) {
     auto before = m_constraints.get_current_pose();
 
     // -------------------------------- Matching -------------------------------- //
     // Match each type of feature
     tuple::for_seq(SEQ, [&](auto I) {
       std::get<I>(m_matcher).template match<I>(std::get<I>(world_map),
-                                               std::get<I>(keypoints), before,
-                                               m_constraints, scan_constraints);
+                                               std::get<I>(keypoints), estimates,
+                                               scan_constraints);
     });
 
     // ---------------------- Semi-Linearized Optimization ---------------------- //

form/optimization/gtsam.hpp

@@ -35,33 +35,34 @@ make it faster when sparsity isn't a concern.
 */
 namespace form {
 
-/// Extension of LM that uses dense optimization instead of sparse bayes tree solver.
+/// @brief Extension of LM that uses dense optimization instead of sparse bayes tree
+/// solver.
 class DenseLMOptimizer : public gtsam::LevenbergMarquardtOptimizer {
 public:
-  /// Constructor
+  /// @brief Constructor
   DenseLMOptimizer(const gtsam::NonlinearFactorGraph &graph,
                    const gtsam::Values &initialValues,
                    const gtsam::LevenbergMarquardtParams &params)
       : LevenbergMarquardtOptimizer(graph, initialValues, params) {}
 
-  /// Solve densely instead of using the sparse solver
+  /// @brief Solve densely instead of using the sparse solver
   gtsam::VectorValues
   solve(const gtsam::GaussianFactorGraph &gfg,
         const gtsam::NonlinearOptimizerParams &params) const override {
     return gfg.optimizeDensely();
   }
 };
 
-/// Factor that forces gtsam factors to use Hessian linearization instead of Jacobian
-/// linearization. This is more efficient when residual sizes are large and the
-/// problem is dense.
+/// @brief Factor that forces gtsam factors to use Hessian linearization instead of
+/// Jacobian linearization. This is more efficient when residual sizes are large and
+/// the problem is dense.
 class DenseFactor : public gtsam::NoiseModelFactor2<gtsam::Pose3, gtsam::Pose3> {
 public:
   DenseFactor(const gtsam::noiseModel::Base::shared_ptr &noiseModel,
               const gtsam::Key i, const gtsam::Key j)
       : gtsam::NoiseModelFactor2<gtsam::Pose3, gtsam::Pose3>(noiseModel, i, j) {}
 
-  // Blatantly stolen from here,
+  // Blatantly stolen from here, just with last line changed:
   // https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/NonlinearFactor.cpp#L152
   boost::shared_ptr<gtsam::GaussianFactor>
   linearize(const gtsam::Values &x) const override {
@@ -85,8 +86,8 @@ class DenseFactor : public gtsam::NoiseModelFactor2<gtsam::Pose3, gtsam::Pose3>
   }
 };
 
-/// A fast isotropic noise model that uses a single sigma value for all dimensions.
-/// The gtsam version allocates a vector of sigmas, which is unnecessary
+/// @brief A fast isotropic noise model that uses a single sigma value for all
+/// dimensions. The gtsam version allocates a vector of sigmas, which is unnecessary
 class FastIsotropic : public gtsam::noiseModel::Gaussian {
   typedef boost::shared_ptr<FastIsotropic> shared_ptr;
 
@@ -138,7 +139,8 @@ class FastIsotropic : public gtsam::noiseModel::Gaussian {
   }
 };
 
-// Wrapper around a gtsam::NoiseModelFactor2 to allow for a fixed FIRST variable
+/// @brief Wrapper around a gtsam::NoiseModelFactor2 to allow for a fixed FIRST
+/// variable
 class BinaryFactorWrapper : public gtsam::NoiseModelFactor1<gtsam::Pose3> {
   using Base = gtsam::NoiseModelFactor1<gtsam::Pose3>;
   using Wrapped = gtsam::NoiseModelFactor2<gtsam::Pose3, gtsam::Pose3>;

form/optimization/matcher.hpp

@@ -19,9 +19,10 @@
 // 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/mapping/map.hpp"
-#include "form/optimization/constraints.hpp"
 #include "form/utils.hpp"
+#include <gtsam/geometry/Pose3.h>
 #include <tbb/concurrent_vector.h>
 #include <tbb/parallel_for.h>
 
@@ -60,14 +61,13 @@ template <typename Point> class Matcher {
   /// @tparam I Index of the constraint type (0 for PlanePoint, 1 for PointPoint)
   /// @param map The voxel map to match against
   /// @param keypoints The keypoints to match
-  /// @param init Initial pose estimate for the scan the keypoints come from
-  /// @param manager The constraint manager containing the poses of all scans
+  /// @param estimates Function that returns the pose estimate for a given FrameIndex
   /// @param scan_constraints Map from FrameIndex to a tuple of constraint vectors
   /// where the matched constraints will be added
   template <int I>
   void match(const VoxelMap<Point> &map, const std::vector<Point> &keypoints,
-             const gtsam::Pose3 &init, const ConstraintManager &manager,
-             tsl::robin_map<FrameIndex, std::tuple<PlanePoint::Ptr, PointPoint::Ptr>>
+             const std::function<gtsam::Pose3(size_t)> &estimates,
+             tsl::robin_map<size_t, std::tuple<PlanePoint::Ptr, PointPoint::Ptr>>
                  &scan_constraints) {
     // Set everything up
     matches.clear();
@@ -76,6 +76,7 @@ template <typename Point> class Matcher {
     }
     matches.reserve(keypoints.size());
     auto max_dist_sqrd = m_params.max_dist_matching * m_params.max_dist_matching;
+    auto init = estimates(keypoints.front().scan);
 
     // Do the matching
     const auto range = tbb::blocked_range{keypoints.cbegin(), keypoints.cend()};
@@ -86,7 +87,7 @@ template <typename Point> class Matcher {
         // move back into local frames
         match.query = *kp;
         if (match.found()) {
-          auto &scan_pose = manager.get_pose(match.point.scan);
+          auto scan_pose = estimates(match.point.scan);
           match.point.transform_in_place(scan_pose.inverse());
         }