ICP Nearest Neighbor

mediumPRO3D ScanningScanningCV: 3D Reconstruction

Implement a function to find the closest point in a target point cloud for each source point, a crucial step in the Iterative Closest Point (ICP) algorithm. This process is essential in 3D scanning and reconstruction to align two point clouds by establishing correspondences between them.

The ICP algorithm relies on the concept of Euclidean distance to measure the proximity between points in 3D space, calculated as $d(p, q) = \sqrt{(p_x - q_x)^2 + (p_y - q_y)^2 + (p_z - q_z)^2}$ . To find the closest point, one must iterate through all target points and determine which point yields the minimum distance for each source point.

Here are the general steps involved:

Iterate over each source point
For each source point, calculate the distance to all target points
Identify the target point with the minimum distance for each source point

d(p, q) = \sqrt{(p_x - q_x)^2 + (p_y - q_y)^2 + (p_z - q_z)^2}

This technique is widely used in computer vision and robotics for 3D reconstruction and object recognition.

Example

Input

find_closest([[0, 0, 0], [1, 0, 0]], [[0.1, 0, 0], [0.9, 0, 0], [5, 5, 5]])

Output

[0, 1]

Reasoning

Inputs: source_points holds two 3D points, $s_0 = (0,0,0)$ and $s_1 = (1,0,0)$ ; target_points holds three, $t_0 = (0.1,0,0)$ , $t_1 = (0.9,0,0)$ and $t_2 = (5,5,5)$ . Each target is identified by its position in the list.
Source $s_0$ : $d(s_0,t_0) = 0.1$ , $d(s_0,t_1) = 0.9$ , and $d(s_0,t_2) = \sqrt{5^2+5^2+5^2} = \sqrt{75} \approx 8.660$ . The smallest is $t_0$ , so its match is index $0$ .
Source $s_1$ : $d(s_1,t_0) = 0.9$ , $d(s_1,t_1) = 0.1$ , and $d(s_1,t_2) = \sqrt{4^2+5^2+5^2} = \sqrt{66} \approx 8.124$ . The smallest is $t_1$ , so its match is index $1$ .
Output: [0, 1], one target index per source point, in source order. It holds indices, not coordinates or distances.
Notice that comparing squared distances ( $0.01$ vs $0.81$ vs $75$ ) gives the same winner, because the square root never changes which value is smallest. Different source points can also share a match: if both sources sat near $t_0$ , both entries of the output would be $0$ .

Constraints

source_points: list of [x, y, z] points
target_points: list of [x, y, z] points
Return list of indices of closest target points

Learn this in

3D ReconstructionCV Study Plan Point Cloud Processing3D Computer Vision

More from CV: 3D Reconstruction

NeRF Ray Samplinghard Surface Integration from Normalsmedium Marching Cubes SDF to Meshmedium ICP Point Cloud Alignmenthard NeRF Positional Encodingmedium

Concept primerPREVIEW

ICP nearest neighbor search is about, for each source point in 3D, finding the closest point in a target point cloud under the standard Euclidean distance. This is the core correspondence step used inside the Iterative Closest Point (ICP) algorithm, which in turn is a standard method for aligning two 3D scans by repeatedly matching points and estimating a rigid transform. In real 3D reconstruction and scanning systems, this nearest-neighbor step is often the main computational bottleneck, so understanding both the brute-force solution and its optimizations (like kd-trees) is important.

Conceptually, each point is just a vector $p = (p_x, p_y, p_z)$ . The Euclidean distance between two points $p$ and $q$ is:

d(p, q) = \sqrt{(p_x - q_x)^2 + (p_y - q_y)^2 + (p_z - q_z)^2}

For nearest-neighbor search, you do not need the square root; the ordering is the same for distance and squared distance. ICP uses these nearest neighbors as correspondences, then separately solves for the rigid transformation (rotation + translation) that best aligns them in a least-squares sense.

2. Algorithm / Approach

For a Medium-difficulty coding problem, the expected approach is usually the direct / brute-force nearest neighbor search:

For each source point:
Scan through all target points
Compute the (squared) Euclidean distance
Keep track of the minimum distance and its index
Return an array of indices, one per source point.

In practical ICP systems with large clouds, this is accelerated with spatial data structures (kd-trees, octrees, kNN structures) to get sublinear queries per source point. But unless the problem statement explicitly asks for it or gives very large constraints, the straightforward double loop is generally acceptable and easier to implement correctly.

3. Step-by-Step Strategy

Understand the inputs and outputs

Input:
source: array of shape [N, 3] (N source points)
target: array of shape [M, 3] (M target points)
Output:
indices: array of length N, where indices[i] is the index (0-based or 1-based depending on problem) of the nearest target point to source[i].

Define a distance helper (optional)

Use squared Euclidean distance to avoid sqrt:

def squared_distance(p, q):
dx = p - q
dy = p - q
dz = p - q
return dx*dx + dy*dy + dz*dz

Loop over source points

For each index i in [0, N):
Get p = source[i].
Initialize:

best_dist = +infinity # a large number
best_j = -1

PRO

Continue the full explanation

You're reading the free preview. Unlock the complete walkthrough, the code editor, test runner and reference solution with Premium.

PRO

Editor locked

The code editor is locked for Pro problems. It is only available for free problems. Please upgrade to gain access to the code editor for all problems.

solution.py

Python 3.13

Test results