ICP Nearest Neighbor
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)=(px−qx)2+(py−qy)2+(pz−qz)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
This technique is widely used in computer vision and robotics for 3D reconstruction and object recognition.
Example:
find_closest([[0, 0, 0], [1, 0, 0]], [[0.1, 0, 0], [0.9, 0, 0], [5, 5, 5]])
[0, 1]
Finding closest targets for 2 source points: Source [0,0,0]: to [0.1,0,0]: d = 0.1 to [0.9,0,0]: d = 0.9 to [5,5,5]: d = 8.66
- → closest is index 0
Source [1,0,0]: to [0.1,0,0]: d = 0.9 to [0.9,0,0]: d = 0.1 ← closest to [5,5,5]: d = 7.81
- → closest is index 1
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
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=(px,py,pz). The Euclidean distance between two points p and q is:
d(p,q)=(px−qx)2+(py−qy)2+(pz−qz)2For 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
Continue the full explanation
You're reading the free preview. Unlock the complete walkthrough, the code editor, test runner and reference solution with Premium.
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.