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 $g(n(p_{kr}))$ is the local gradient direction at the matched pixel. The Huber-weighted ICP objective to be minimized is

$$E^E_{kr}(\xi) = \sum_{p_r \in S_r^E} w_{p_r} \rho\left( g(n(p_{kr}))^\top [p_{kr} - n(p_{kr})] \right).$$

The optimization proceeds in a multi-scale image pyramid, using Gauss-Newton updates $\delta\eta \in \mathbb{R}^6$ with pose composition $\xi \leftarrow \exp(\delta\eta) \circ \xi$, 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 $g_\perp$, but the search interval is bounded by a probabilistic uncertainty-driven radius.

The search length is set as

$$\lambda_{1/2} = k_p \sigma_{p\perp g} + k_\mu \sigma_\mu |\sin\theta|,$$

where $\theta$ is the angle between the epipolar line and edge normal, $\sigma_{p\perp g}$ is the projected uncertainty along $g_\perp$, $\sigma_\mu$ is the depth-disparity uncertainty, and $k_p$, $k_\mu \approx 1$ are empirically tuned gains.

Candidate matches $q_\ell$ are extracted along $g_\perp$ within $[\pm \lambda_{1/2}]$, each pre-warped according to the current pose and depth hypothesis, with a patch of image-gradient magnitudes $F$ extracted. The L$_2$ cost for each candidate is

$c_\ell = \| F_k(q_\ell) - F_r(p_r) \|_2^2,$

and the match $q^* = {\arg\min}_\ell c_\ell$ 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 $S = \{ p^* + \lambda g_\perp \}$ and the epipolar line $L = \{ p + \mu l \}$ yields: $$\lambda(p, \mu) = \langle p^* - p, g_\perp \rangle + \mu \langle l, g_\perp \rangle = e_{p\perp g} + \mu \sin\theta,$$ and the variance: $$\sigma_\lambda^2 = \sigma_{p\perp g}^2 + \sigma_\mu^2 \sin^2\theta,$$ with an upper bound $$\sigma_\lambda \leq \sigma_{p\perp g} + \sigma_\mu |\sin\theta|$$.

The disparity $\mu$ and its variance are: $$\mu(p) = \frac{\langle p^* - p, g \rangle}{\langle g, l \rangle} = \frac{e_{p \parallel g}}{\cos\theta}, \qquad \sigma_\mu^2 = \frac{\sigma_{p\parallel g}^2}{\cos^2\theta},$$ where the directional uncertainties are decomposed using the principal axes $(v_1, v_2)$ and variances $(\sigma_1, \sigma_2)$. The depth confidence is defined by $C_d = 1/\sigma_\mu$.

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

$C_m = \frac{1}{\sum_\ell (c_\ell - c_*)^2},$

where $c_*$ is the minimal patch cost in the current search. If $C_m < \tau_m$, the patch size is incremented $S_p \leftarrow \min(S_p + 2, S_{\max})$ 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 $C_d$), 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 $\{ \xi_i \}$ and depths $\{ d_j \}$. The cost per correspondence incorporates:

• the point-to-tangent residual,
• a reprojection residual: $$E^R_{ij} = \rho\left(\| \pi(R_{ij} \pi^{-1}(p_r, d_r) + t_{ij}) - q^* \|_2\right),$$
• an optional photometric gradient-consistency term: $E^P_{ij} = \rho(\| F_j(\pi(...)) - F_r(p_r) \|_2).$ The objective minimized is: $$\sum_{i,r} w_{ir}^E E^E_{ir} + \sum_{i,r} w_{ir}^R E^R_{ir} + \sum_{i,r} w_{ir}^P E^P_{ir},$$ 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 $1.5$–$2$ cm/m), with core processing times of $\sim20$ ms (matching) and $\sim50$ 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 $6$ cm/m in winter). Under $3\times$ frame subsampling (fast motion), drift degraded only slightly, while other approaches displayed $2$–$3\times$ increased failure. Full system runtime was approximately $80$ ms/frame for tracking plus $200$ ms/frame for mapping with a laptop and GPU edge-detector.

KITTI benchmark results (after scale correction) reported mean Absolute Trajectory Error (ATE) of $11.5$ m over sequences $00$–$09$ (matching ORB-SLAM2 and outperforming DSO; $11.5$ m vs $12.1$ 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.