二维点云ICP的python实现

描述

使用python语言来实现二维点云的ICP算法
二维点云ICP算法原理及推导，请见我的另外一篇博客
二维点云ICP原理推导

特点说明：

1. ICP算法中的loss计算方式，可以根据自己实际需要来调整。我这里使用的是，目标点云A中的某个点 a，从源点云 B 中找到距离点 a 最近的点 b，总的loss就是这些a-b距离之和
2. 正常的loss计算方式，应该是从源点云B中的每个点 b，去目标点云A里寻找最近点
3. loss的定义方式一定要好好思考，结合自己的实际需要，不要只会把我下面的代码搬过去，因为可能不work

代码

import numpy as np
import math
import matplotlib.pyplot as plt

# 求出两个点之间的向量角度，向量方向由点1指向点2
def getThetaOfTwoPoints(x1, y1, x2, y2):
    return math.atan2(y2-y1, x2-x1)

# 求出两个点的距离
def getDistOfTwoPoints(x1, y1, x2, y2):
    return math.sqrt(math.pow(x2-x1, 2) + math.pow(y2-y1, 2))

# 在pt_set点集中找到距(p_x, p_y)最近点的id
def getClosestID(p_x, p_y, pt_set):
    id = 0
    min = 10000000
    for i in range(pt_set.shape[1]):
        dist = getDistOfTwoPoints(p_x, p_y, pt_set[0][i], pt_set[1][i])
        if dist < min:
            id = i
            min = dist
    return id

# 求出两个点集之间的平均点距
def DistOfTwoSet(set1, set2):
    loss = 0;
    for i in range(set1.shape[1]):
        id = getClosestID(set1[0][i], set1[1][i], set2)
        dist = getDistOfTwoPoints(set1[0][i], set1[1][i], set2[0][id], set2[1][id])
        loss = loss + dist
    return loss/set1.shape[1]

# ICP核心代码
def ICP(sourcePoints, targetPoints):
    A = targetPoints
    B = sourcePoints

    iteration_times = 0
    dist_now = 1
    dist_improve = 1
    dist_before = DistOfTwoSet(A, B)
    while iteration_times < 10 and dist_improve > 0.001:
        x_mean_target = A[0].mean()
        y_mean_target = A[1].mean()
        x_mean_source = B[0].mean()
        y_mean_source = B[1].mean()

        A_ = A - np.array([[x_mean_target], [y_mean_target]])
        B_ = B - np.array([[x_mean_source], [y_mean_source]])

        w_up = 0
        w_down = 0
        for i in range(A_.shape[1]):
            j = getClosestID(A_[0][i], A_[1][i], B_)
            w_up_i = A_[0][i]*B_[1][j] - A_[1][i]*B_[0][j]
            w_down_i = A_[0][i]*B_[0][j] + A_[1][i]*B_[1][j]
            w_up = w_up + w_up_i
            w_down = w_down + w_down_i

        theta = math.atan2(w_up, w_down)
        x = x_mean_target - math.cos(theta)*x_mean_source - math.sin(theta)*y_mean_source
        y = y_mean_target + math.sin(theta)*x_mean_source - math.cos(theta)*y_mean_source
        R = np.array([[math.cos(theta), math.sin(theta)], [-math.sin(theta), math.cos(theta)]])

        B = np.matmul(R, B) + np.array([[x], [y]])

        iteration_times = iteration_times + 1
        dist_now = DistOfTwoSet(A, B)
        dist_improve = dist_before - dist_now
        print("迭代第"+str(iteration_times)+"次, 损失是"+str(dist_now)+",提升了"+str(dist_improve))
        dist_before = dist_now

    return B