JointPairwiseRegistration.hpp

Go to the documentation of this file.

// SPDX-FileCopyrightText: 2025 Povl Filip Sonne-Frederiksen
//
// SPDX-License-Identifier: GPL-3.0-or-later
//
// Joint Pairwise Registration (JPR) for within-scan pose refinement.
//
// Reimplements the C++ core of the Joint Pairwise Registration method from
//   Q. Huang et al., "Uncertainty Quantification for Multi-Scan Registration",
//   ACM SIGGRAPH 2020 (MIT-licensed reference implementation:
//   https://github.com/huangqx/MultiScanRegistration).
//
// The reference is MATLAB/MEX research code operating on object scans; this is
// a native C++ port adapted to refine the per-frame sensor poses of a single
// ReUseX scan. Each sensor frame is treated as one JPR "scan": its depth is
// back-projected to an optical-frame surfel set, its pose is seeded from the
// stored (RTABMap) world pose, and all frame poses are jointly refined by
// minimizing point-to-plane distances between overlapping frame pairs.
//
// Deviations from the reference (documented intentionally):
//  - Pose increments use a minimal se(3) twist parametrization solved by a
//    sparse Gauss-Newton step, rather than the reference's 12-dim [t; vec(R)].
//  - Nearest-neighbour search uses pcl::KdTreeFLANN instead of the ANN library.
//  - Gauge freedom is fixed by a soft prior to the seed poses (and/or an
//    optional hard-anchored frame), which also prevents drift away from the
//    already-good RTABMap poses we are refining.
 
#pragma once
 
#include "reusex/segmentation/Surfel.hpp"
#include "reusex/segmentation/surfel_extraction.hpp"
 
#include <vector>
 
namespace reusex {
class ProjectDB;
}
 
namespace reusex::geometry {

struct JprParams {
  int max_iterations = 20;         
  int neighbor_window = 5;         
  float max_corr_distance = 0.10f; 
  float normal_angle_threshold =
      45.f; 
  float robust_width =
      0.05f; 
 
  enum class Kernel { welsch, huber };
  Kernel kernel = Kernel::welsch;
 
  float prior_weight =
      1.0f;              
  int anchor_frame = -1; 
  float convergence_eps = 1e-4f; 
 
  SurfelExtractionParams surfel; 
};

struct JprResult {
  int frames = 0;           
  int iterations = 0;       
  double initial_rms = 0.0; 
  double final_rms = 0.0;   
};

class JointPairwiseRegistration {
    public:
  explicit JointPairwiseRegistration(JprParams params);

  JprResult refine(std::vector<FrameSurfels> &frames) const;
 
    private:
  JprParams params_;
};

JprResult refine_sensor_poses(ProjectDB &db, const JprParams &params,
                              bool dry_run = false);
 
} // namespace reusex::geometry