Python实现LiDAR点云配准融合以重建3D岩石模型的技术求助

我正在尝试对齐两组分别代表岩石上部和下部的点云，二者共享岩石侧面的公共区域。目标是配准融合这些点云，使公共区域精准匹配，从而准确重建并3D展示该岩石。RevoScan5可完成此操作，但我希望通过Python实现配准自动化。

期望对齐效果：两组点云的公共区域精准重合，完整还原岩石的3D结构。

提前感谢您的帮助！

预处理步骤 #

首先，我对点云进行了下采样并去除离群点，随后对底部扫描点云进行旋转平移以实现粗配准，便于上下部分匹配。

配准前状态：源点云（上部）与目标点云（下部）的公共区域未对齐，呈现上下分离的形态。

尝试的配准方法 #

1. 基于Open3D的全局配准方法（RANSAC+ICP） #

参考Open3D官方全局配准教程实现，调整多种参数后，始终得到较低的适配度分数，点云匹配效果差，无法重建出完整的3D岩石模型。更换不同岩石样本尝试后仍未成功。

RANSAC配准结果：点云匹配效果差，公共区域未重合。
ICP细化后结果：匹配效果仍不理想，无法重建完整模型。

def execute_global_registration(source_down, target_down, source_fpfh,
                                target_fpfh, voxel_size):
    distance_threshold = voxel_size * 1.5
    print(":: RANSAC registration on downsampled point clouds.")
    print("   Since the downsampling voxel size is %.3f," % voxel_size)
    print("   we use a liberal distance threshold %.3f." % distance_threshold)
    result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
        source_down, target_down, source_fpfh, target_fpfh, True,
        distance_threshold,
        o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
        3, [
            o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(
                0.9),
            o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(
                distance_threshold)
        ], o3d.pipelines.registration.RANSACConvergenceCriteria(1000000000, 0.999))
    return result

def draw_registration_result(source, target, transformation, window_title):
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.706, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    source_temp.transform(transformation)
    o3d.visualization.draw_geometries([source_temp, target_temp], window_name=window_title)
    
#draw_registration_result(source, target, np.identity(4))

def preprocess_point_cloud(pcd, voxel_size):
    #print(":: Downsample with a voxel size %.3f." % voxel_size)
    #pcd_down = pcd.voxel_down_sample(voxel_size) --> already downsampled in previous section

    radius_normal = voxel_size * 1 #initially 2
    print(":: Estimate normal with search radius %.3f." % radius_normal)
    pcd.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=500))

    radius_feature = voxel_size * 5
    print(":: Compute FPFH feature with search radius %.3f." % radius_feature)
    pcd_fpfh =o3d.pipelines.registration.compute_fpfh_feature(pcd, o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
    return pcd, pcd_fpfh

def refine_registration(source, target, source_fpfh, target_fpfh, voxel_size):
    distance_threshold = voxel_size * 0.6
    result = o3d.pipelines.registration.registration_icp(
        source, target, distance_threshold, result_ransac.transformation,
        o3d.pipelines.registration.TransformationEstimationPointToPlane())
    return result

source_down, source_fpfh = preprocess_point_cloud(source, voxel_size)
target_down, target_fpfh = preprocess_point_cloud(target, voxel_size)


result_ransac = execute_global_registration(source_down, target_down,
                                            source_fpfh, target_fpfh,
                                            voxel_size)

o3d.visualization.draw_geometries([source, target], window_name='Before registration')
print('\n')
print(result_ransac)
draw_registration_result(source, target, result_ransac.transformation, 'Ransac result')

result_icp = refine_registration(source, target, source_fpfh, target_fpfh,
                                 voxel_size)
print('\n')
print(result_icp)
draw_registration_result(source, target, result_icp.transformation, 'ICP result')

2. 自定义ICP算法 #

该方法基于顶点实现，但ICP后，源点云与目标点云的错位程度比配准前更严重。

自定义ICP结果：源点云与目标点云错位程度加剧。

import numpy as np
from sklearn.neighbors import NearestNeighbors
import matplotlib.pyplot as plt

# Function to find the nearest neighbors between source and destination point clouds
def find_nearest_neighbors(src, dst):
    neigh = NearestNeighbors(n_neighbors=1)
    neigh.fit(dst)
    distances, indices = neigh.kneighbors(src, return_distance=True)
    return distances.flatten(), indices.flatten()

# Function to compute the best-fit transform between the aligned source and destination points
def best_fit_transform(src, dst, weights=None):
    centroid_src = np.average(src, axis=0, weights=weights)
    centroid_dst = np.average(dst, axis=0, weights=weights)
    src_centered = src - centroid_src
    dst_centered = dst - centroid_dst
    H = np.dot((src_centered * weights[:, None]).T, dst_centered)
    U, _, Vt = np.linalg.svd(H)
    R = np.dot(Vt.T, U.T)
    t = centroid_dst - np.dot(centroid_src, R)
    return R, t

# The main ICP algorithm
def icp(src, dst, max_iterations=10000, tolerance=1e-6, reject_ratio=0.2):
    prev_error = 0
    for i in range(max_iterations):
        distances, indices = find_nearest_neighbors(src, dst)
        sorted_idx = np.argsort(distances)
        reject_count = int(reject_ratio * len(sorted_idx))
        src_filtered = src[sorted_idx[:-reject_count]]
        indices_filtered = indices[sorted_idx[:-reject_count]]
        dst_matched = dst[indices_filtered]
        weights = np.exp(-distances[sorted_idx[:-reject_count]] ** 2)
        R, t = best_fit_transform(src_filtered, dst_matched, weights)
        src = np.dot(src, R.T) + t
        mean_error = np.mean(distances)
        if np.abs(prev_error - mean_error) < tolerance:
            break
        prev_error = mean_error
    return src

# Function to plot point clouds
def plot_point_clouds(src, dst, title):
    fig = plt.figure(figsize=(15, 10))
    ax = fig.add_subplot(111, projection='3d')
    ax.scatter(src[:, 0], src[:, 1], src[:, 2], c='r', marker='o', label='Source', s=s)
    ax.scatter(dst[:, 0], dst[:, 1], dst[:, 2], c='b', marker='^', label='Destination', s=s)
    ax.set_title(title)
    ax.legend()
    plt.show()

# Extract the vertices (3D points)
vertices_source = np.asarray(source.points)

# Extract the vertices (3D points)
vertices_target = np.asarray(target.points)

# Plot point clouds before ICP
plot_point_clouds(vertices_source, vertices_target, 'Before ICP')

# Apply ICP to align the source point cloud to the destination
aligned_src = icp(vertices_source, vertices_target)

# Plot point clouds after ICP
plot_point_clouds(aligned_src, vertices_target, 'After ICP')

3. CPD算法 #

最后尝试了CPD方法，但出现LinAlgError: SVD did not converge错误，表明模型未收敛。大幅增加迭代次数（max_iterations）后，仍未收敛。

# Function to compute Gaussian kernel matrix between two sets of vectors
def gaussian_kernel(x, y, beta):
    """Computes Gaussian kernel between x and y with width beta"""
    return np.exp(-beta * np.sum((x[:, None] - y[None, :]) ** 2, axis=2))

# Main function for Coherent Point Drift (CPD) algorithm
def cpd_rigid(X, Y, max_iterations=200, tolerance=1e-6, w=0.0, beta=2.0):
    """Performs rigid CPD between source X and target Y"""
    N, D = X.shape  # Number of points and dimensions in source
    M = Y.shape[0]  # Number of points in target

    # Initialize sigma^2
    sigma2 = np.sum((X - np.mean(X, axis=0)) ** 2) + np.sum((Y - np.mean(Y, axis=0)) ** 2)
    sigma2 /= (D * (N + M))

    TY = np.copy(Y)  # Transformed target point cloud
    P = np.zeros((M, N))  # Matrix for storing correspondence probabilities
    prev_error = 0  # Variable to store error from previous iteration

    # Iterative optimization
    for i in range(max_iterations):
        # E-step: Compute correspondence probabilities using Gaussian kernel
        P = gaussian_kernel(X, TY, 1 / (2 * sigma2))
        C = np.sum(P, axis=0)
        P /= C[None, :]  # Normalize probabilities

        # M-step: Update the transformation parameters
        Np = np.sum(P)  # Normalization factor
        muX = np.sum(np.dot(P, X), axis=0) / Np  # Weighted mean of source points
        muY = np.sum(TY * np.sum(P, axis=1)[:, None], axis=0) / Np  # Weighted mean of target points

        # Centered matrices
        X_hat = X - muX
        Y_hat = TY - muY

        # Compute new transformation using SVD
        A = np.dot(X_hat.T, np.dot(np.diag(np.sum(P, axis=0)), Y_hat)) / Np
        U, _, V = np.linalg.svd(A)
        R = np.dot(U, V)
        t = muX - np.dot(R, muY)

        # Update the model to transform the target point cloud
        TY = np.dot(R, Y.T).T + t

        # Update sigma^2
        error = np.sum(np.sum(P * np.sum((X[:, None] - TY[None, :]) ** 2, axis=2), axis=0)) / Np
        sigma2 = (1 - w) * error / (D * Np)

        # Check for convergence based on the change in error
        if np.abs(prev_error - error) < tolerance:
            break
        prev_error = error

    return TY

# Function to plot point clouds
def plot_point_clouds(src, dst, title):
    fig = plt.figure(figsize=(15, 10))
    ax = fig.add_subplot(111, projection='3d')
    ax.scatter(src[:, 0], src[:, 1], src[:, 2], c='r', marker='o', label='Source', s=s)
    ax.scatter(dst[:, 0], dst[:, 1], dst[:, 2], c='b', marker='^', label='Destination', s=s)
    ax.set_title(title)
    ax.legend()
    plt.show()

# Assuming you have matrices vertices_s and vertices_t
num_rows_to_remove = vertices_t.shape[0] - vertices_s.shape[0]

# Slice vertices_t to remove the extra rows
vertices_t_trimmed = vertices_t[:-num_rows_to_remove, :]

# Plot point clouds before alignment
plot_point_clouds(vertices_s, vertices_t_trimmed, 'Before CPD')

# Perform CPD alignment
aligned_X = cpd_rigid(vertices_s, vertices_t_trimmed)

# Plot point clouds after alignment
plot_point_clouds(aligned_X, vertices_t_trimmed, 'After CPD')

内容的提问来源于stack exchange，提问作者Anna