Edge Detection-Based ICP (ED-ICP) Algorithm

• Edge Detection-Based ICP (ED-ICP) is a method for robust monocular visual odometry that integrates edge alignment within an ICP framework.
• The algorithm employs a coarse-to-fine, uncertainty-driven edge-guided data association along with adaptive patch sizing for precise real-time pose estimation.
• Quantitative evaluations on synthetic and real-world datasets demonstrate reduced drift and failure rates compared to classical methods under challenging illumination and motion.

Edge Detection-Based ICP (ED-ICP) is a methodology for monocular visual odometry that exploits edge feature alignment within an Iterative Closest Point (ICP) framework, augmented with a coarse-to-fine, uncertainty-driven edge-guided data association scheme. The approach is designed for robust camera tracking under challenging illumination and large motion, enabling resilient and accurate real-time pose estimation with improved performance over classical point-based and direct methods (Wu et al., 2019).

1. ICP-Based Edge Registration: Front-End

The front-end initiates by extracting a set of prominent 2D edges from the incoming image using detectors such as Canny, Structured Forest (SE), or Holistically-Nested Edge Detection (HED). Each reference-frame edge pixel $p_r$ is characterized by its position $p_r \in \Omega \subset \mathbb{R}^2$ and local gradient direction $g(p_r)$, locally representing the edge as a 1D curve with normal $g(p_r)$ and tangent $g_\perp$.

Given a relative pose estimate $\xi \in \mathfrak{se}(3)$ between frames, each reference edge pixel is back-projected using its inverse-depth $d_r$ and re-projected into the current frame with

$p_{kr} = \pi(R_{kr} \pi^{-1}(p_r, d_r) + t_{kr}).$

The nearest-neighbor edge pixel $n(p_{kr})$ in the current frame is found by minimizing the Euclidean distance. The matching cost employs a point-to-tangent residual: $$r_i(\xi) = g(n(p_{kr}))^\top \left[p_{kr} - n(p_{kr})\right],$$ where $p_r \in \Omega \subset \mathbb{R}^2$0 is the local gradient direction at the matched pixel. The Huber-weighted ICP objective to be minimized is

$p_r \in \Omega \subset \mathbb{R}^2$1

The optimization proceeds in a multi-scale image pyramid, using Gauss-Newton updates $p_r \in \Omega \subset \mathbb{R}^2$2 with pose composition $p_r \in \Omega \subset \mathbb{R}^2$3, providing robust but coarse motion estimation and initial data association.

2. Coarse-to-Fine Edge-Guided Data Association

Following the ICP phase, refined edge correspondence is conducted to address the partial observability induced by lack of unique edge descriptors. This proceeds as a 1D template-matching search along the tangent direction $p_r \in \Omega \subset \mathbb{R}^2$4, but the search interval is bounded by a probabilistic uncertainty-driven radius.

The search length is set as

$p_r \in \Omega \subset \mathbb{R}^2$5

where $p_r \in \Omega \subset \mathbb{R}^2$6 is the angle between the epipolar line and edge normal, $p_r \in \Omega \subset \mathbb{R}^2$7 is the projected uncertainty along $p_r \in \Omega \subset \mathbb{R}^2$8, $p_r \in \Omega \subset \mathbb{R}^2$9 is the depth-disparity uncertainty, and $g(p_r)$0, $g(p_r)$1 are empirically tuned gains.

Candidate matches $g(p_r)$2 are extracted along $g(p_r)$3 within $g(p_r)$4, each pre-warped according to the current pose and depth hypothesis, with a patch of image-gradient magnitudes $g(p_r)$5 extracted. The L$g(p_r)$6 cost for each candidate is

$g(p_r)$7

and the match $g(p_r)$8 is selected as the refined correspondence.

3. Geometric Uncertainty Analysis and Dynamic Search Bounding

The search interval and depth uncertainty metrics are analytically derived via point-to-edge geometric uncertainty analysis. The intersection of the edge direction $g(p_r)$9 and the epipolar line $g(p_r)$0 yields: $g(p_r)$1 and the variance: $g(p_r)$2 with an upper bound $g(p_r)$3.

The disparity $g(p_r)$4 and its variance are: $g(p_r)$5 where the directional uncertainties are decomposed using the principal axes $g(p_r)$6 and variances $g(p_r)$7. The depth confidence is defined by $g(p_r)$8.

4. Match Confidence and Adaptive Patch Sizing

To resolve ambiguities on low-texture or flat edges, the algorithm computes the Attainable Maximum Likelihood (AML) confidence

$g(p_r)$9

where $g_\perp$0 is the minimal patch cost in the current search. If $g_\perp$1, the patch size is incremented $g_\perp$2 to include additional gradient structure and re-evaluate the candidates. If confidence remains insufficient, matching falls back to the initial ICP result, and for the lowest confidence (small $g_\perp$3), the corresponding depth estimate is held fixed in bundle adjustment.

5. Bundle Adjustment and Joint Optimization

Refined correspondences with associated depth and confidence metrics are aggregated in a local window for a joint optimization over all poses $g_\perp$4 and depths $g_\perp$5. The cost per correspondence incorporates:

• the point-to-tangent residual,
• a reprojection residual: $g_\perp$6
• an optional photometric gradient-consistency term: $g_\perp$7 The objective minimized is: $g_\perp$8 using frameworks such as g2o or custom Levenberg-Marquardt solvers. Corrrespondences with low depth-confidence are included with depths fixed to preserve mapping integrity.

6. Quantitative Evaluation and Comparative Performance

ED-ICP was evaluated on synthetic and real-world datasets with varying photometric noise and illumination. On vKITTI with day/night variance, it attained the lowest translational drift (approximately $g_\perp$9–$\xi \in \mathfrak{se}(3)$0 cm/m), with core processing times of $\xi \in \mathfrak{se}(3)$1 ms (matching) and $\xi \in \mathfrak{se}(3)$2 ms (ICP) per frame. Classical Lucas-Kanade and census-based optical flows were outperformed both in accuracy and speed, especially under high inter-frame displacement.

On the real-world Symphony-Lake dataset (1.5M images, strong sun-glare, and auto-exposure shifts), ED-ICP achieved the lowest failure rate (1–3% per survey) and low drift (as little as $\xi \in \mathfrak{se}(3)$3 cm/m in winter). Under $\xi \in \mathfrak{se}(3)$4 frame subsampling (fast motion), drift degraded only slightly, while other approaches displayed $\xi \in \mathfrak{se}(3)$5–$\xi \in \mathfrak{se}(3)$6 increased failure. Full system runtime was approximately $\xi \in \mathfrak{se}(3)$7 ms/frame for tracking plus $\xi \in \mathfrak{se}(3)$8 ms/frame for mapping with a laptop and GPU edge-detector.

KITTI benchmark results (after scale correction) reported mean Absolute Trajectory Error (ATE) of $\xi \in \mathfrak{se}(3)$9 m over sequences $d_r$0–$d_r$1 (matching ORB-SLAM2 and outperforming DSO; $d_r$2 m vs $d_r$3 m ATE).

7. Significance and Methodological Contributions

ED-ICP integrates ICP-based edge registration with a fast, analytically bounded 1D photometric refinement to maximize robustness against illumination and geometric perturbations in monocular visual odometry. Its coarse-to-fine data association pipeline, guided by geometric and photometric uncertainty analysis, yields superior performance in complex conditions (illumination change, large motion). This suggests a broader applicability in low-texture or challenging lighting regimes.

By coupling uncertainty-driven correspondence search with dynamic patch size adaptation and rigorous back-end optimization, ED-ICP achieves resilience and efficiency not available in classical direct or feature-based pipelines (Wu et al., 2019). This hybrid approach is significant for real-time VO systems requiring high reliability under adverse environmental variation.