多角度点云先配准后拼接--小白自学全过程

        我手里的数据偏移角度是已知的，按理说可以直接用刚性变换矩阵转变后合并。但我自己尝试之后发现，要么背景平面会重建成“千层饼”要么背景是平面，而前面物体出现“多层平移”如下所示：

平板平，但是物体明显重影

千层饼，图像也有重影

        千层饼问题通过调整旋转矩阵解决了，但是物体的重影我不知道怎么解决，看起来可以调整平移矩阵，但我肉眼看的话难免不精确。因此我决定直接放弃已知的角度信息，通过点云配准找到旋转矩阵R和平移矩阵T，以下为详细过程：

点云配准、拼接概念综述_点云拼接-CSDN博客

        这篇博客中介绍了粗配准和精配准两种方法，因为我的点云本来距离已经很相近了，所以我决定直接使用精配准，即迭代最近点算法（ICP算法）

原理大家自行搜索，我这里只写代码过程：

1. 加载目标点云数据：

用 load_point_cloud 函数从文件加载目标点云数据。

def load_point_cloud(file_path):
    """加载点云数据"""
    points = np.loadtxt(file_path).T
    return points

2. 定义最近邻计算

调用scipy中的KDTree

def nearest_neighbor(src, dst):
    """Find the nearest (minimum distance) neighbors"""
    tree = KDTree(dst.T)
    distances, indices = tree.query(src.T)
    return distances, indices

3.定义icp函数：

icp 函数接收四个参数：

• src: 源点云数据。
• dst: 目标点云数据。
• init_pose (可选): 源点云相对于目标点云的初始姿态，默认为 None，表示使用单位矩阵作为初始姿态。
• max_iterations (可选): 最大迭代次数，默认为50次。
• tolerance (可选): 收敛条件的容差值，默认为0.001。

1. T：未传入初始姿态时初始化一个4x4的单位矩阵作为变换矩阵

2. prev_error：用于存储前一次迭代的均方误差

3. apply_transformation(src, T) ：使用当前的变换矩阵T变换源点云

4. 寻找变换后的源点云 src_transformed 和目标点云 dst 之间的最近邻点，返回距离 distances 和索引 indices

5.计算变换后的源点云和目标点云之间的协方差矩阵 cov，使用奇异值分解（SVD）计算最优旋转矩阵 R，将旋转矩阵 R 赋值给 T 的前3x3部分，将平移向量 t 赋值给 T 的前3行第4列。

def icp(src, dst, init_pose=None, max_iterations=50, tolerance=0.001):
    """
    The Iterative Closest Point algorithm.
    
    Args:
        src: Source point cloud.
        dst: Destination point cloud.
        init_pose: Initial pose of the source point cloud relative to the destination point cloud.
        max_iterations: Maximum number of iterations.
        tolerance: Tolerance value to terminate the algorithm.
    
    Returns:
        final_transformation: Final transformation matrix from source to destination.
        mean_error: Mean square error after convergence.
    """
    # Initialize the transformation matrix
    if init_pose is None:
        T = np.identity(4)
    else:
        T = init_pose

    prev_error = 0

    for _ in range(max_iterations):
        # Transform the source point cloud using the current transformation
        src_transformed = apply_transformation(src, T)

        # Find the nearest neighbors between the transformed source and the destination point cloud
        distances, indices = nearest_neighbor(src_transformed, dst)

        # Compute the mean square error
        mean_error = np.mean(distances)
        
        # Check if the error has converged
        if abs(prev_error - mean_error) < tolerance:
            break
        
        prev_error = mean_error

        # Compute the centroid of the source and destination point clouds
        centroid_src = np.mean(src_transformed, axis=1)
        centroid_dst = np.mean(dst[:, indices], axis=1)

        # Center the point clouds
        src_centered = src_transformed - centroid_src[:, np.newaxis]
        dst_centered = dst[:, indices] - centroid_dst[:, np.newaxis]

        # Compute the covariance matrix
        cov = src_centered @ dst_centered.T

        # Compute the optimal rotation matrix
        U, S, Vt = np.linalg.svd(cov)
        R = Vt.T @ U.T

        # Ensure that the rotation matrix is proper
        if np.linalg.det(R) < 0:
            Vt[2, :] *= -1
            R = Vt.T @ U.T

        # Compute the optimal translation vector
        t = centroid_dst - R @ centroid_src

        # Construct the transformation matrix
        T[:3, :3] = R
        T[:3, 3] = t

    return T, mean_error

4. 对原点云做刚性变换使其坐标系与目标点云一致

def apply_transformation(points, transformation_matrix):
    """Apply a transformation matrix to the points"""
    hom_points = np.vstack((points, np.ones((1, points.shape[1]))))
    transformed_hom_points = transformation_matrix @ hom_points
    transformed_points = transformed_hom_points[:3, :] / transformed_hom_points[3, :]
    return transformed_points

5.把各个角度变换后的点云数据和目标点云数据合并

实现最终目的

if __name__ == "__main__":
    # 源点云所在的目录
    src_dir = r'G:\pointclouds\alldata'

    filenames = [f'point_cloud_{i}.xyz' for i in range(67, 89, 2) if i != 77]

    src_files = [os.path.join(src_dir, name) for name in filenames]

    # 目标点云文件路径
    dst_file = r'G:\pointclouds\alldata\point_cloud_77.xyz'

    # 加载目标点云数据
    dst = load_point_cloud(dst_file)
    
    # 存储所有变换矩阵
    transformations = []
    transformed_points_list = []

    # 对每个源点云进行配准
    for src_file in src_files:
        # 加载源点云数据
        src = load_point_cloud(src_file)
        # 使用ICP算法来配准源点云和目标点云
        final_transformation, error = icp(src, dst)
        # 输出变换矩阵和均方误差
        print(f"Transformation Matrix for {src_file}:")
        print(final_transformation)
        print(f"Mean Square Error: {error}\n")
        # 存储变换矩阵
        transformations.append(final_transformation)
        # 将源点云变换到目标点云的空间内
        transformed_points = apply_transformation(src, final_transformation)
        transformed_points_list.append(transformed_points)

    # 合并所有变换后的点云
    merged_points = np.hstack(transformed_points_list)
    # 保存合并后的点云数据
    output_file = r'G:\YJY\test\test1\deep-rs-Master\base_dir\dataset_root\PUNnet\pointclouds\merged_point_cloud_1_4.xyz'
    np.savetxt(output_file, merged_points.T, fmt='%.6f')