python做的多激光雷达外参标定程序(初版本完整版见专栏)

点云-激光雷达-Slam-三维牙齿

已于 2023-05-21 06:40:47 修改

阅读量1.4k

点赞数

分类专栏：点云及图像-免费文章标签： python 开发语言机器学习

于 2022-07-25 10:54:12 首次发布

本文链接：https://blog.csdn.net/a394467238/article/details/125969756

版权

点云及图像-免费专栏收录该内容

71 篇文章 29 订阅

订阅专栏

这篇博客介绍了一个使用Python编写的激光雷达标定程序，替代了传统的ROS框架下的C++实现。该程序依赖简单，可在Windows系统上运行，支持手动标定和自动配准，提高标定精度。标定流程包括抽取点云数据、手动调整点云位姿、利用ICP算法找到最优外参矩阵。用户可以通过键盘控制点云的旋转和平移，进行实时调整。程序提供了详细的操作说明和部分关键代码展示。

摘要由CSDN通过智能技术生成

查阅了一番资料和现有的代码后发现，现在的多个激光雷达之间的标定程序都是ROS框架下面的，并且都是C++代码，需要安装的依赖也比较复杂，于是自己写了一个python版本的标定程序，依赖非常简单，Windows系统也可以运行。并且代码简单，扩展性比较好，后续想要同时标定2个以上的雷达也很方便拓展；

它能够解决上述的三个问题:

通过手动标定加自动配准可以更精确的标定
标定的参数包含了各个方向的角度和位移
可以实时手动调整点云的位姿，效率较高

标定流程为：

抽取每个雷达的点云数据的一帧数据保存为PCD文件，作为标定的原始数据
先手动标定两个雷达的点云，可以连续任意角度旋转，平移，效率较高，得到标定矩阵的初始值
根据标定的初始值，利用ICP算法找到最优的标定矩阵，使得B点云与A点云完全重合，得到最优的外参标定矩阵

# 通过以下按键来调整点云位姿:

# q,a 偏航角调整

# w,s roll 翻滚角调整

# e,d pitch 俯仰角调整

# r,f X 轴调整

# t,g Y 轴调整

# y,h Z 轴调整

原始未标定数据

标定后的数据

程序如下，只放出了手动标定的部分

# coding:utf-8

import open3d as o3d
import numpy as np
import math
import copy

# 第一版本使用角度转换的旋转矩阵，
# 然后把每次的旋转角度加起来，累计；
# 或者把每次的旋转矩阵累乘，都不是最后的正确结果，很奇怪
# 第二版本改为使用矩阵的逆矩阵，没问题了，应该是矩阵的旋转顺序有影响



# 通过以下按键来调整点云位姿:
# q,a 偏航角调整
# w,s roll 翻滚角调整
# e,d pitch 俯仰角调整
# r,f  X 轴调整
# t,g  Y 轴调整
# y,h  Z 轴调整


def eulerAnglesToRotationMatrix(theta):
    rx = np.asarray([[1.0, 0.0, 0.0],
                     [0.0, math.cos(theta[0]), -math.sin(theta[0])],
                     [0.0, math.sin(theta[0]), math.cos(theta[0])]])

    ry = np.asarray([[math.cos(theta[1]), 0.0, math.sin(theta[1])],
                     [0.0, 1.0, 0.0],
                     [-math.sin(theta[1]), 0.0, math.cos(theta[1])]])

    rz = np.asarray([[math.cos(theta[2]), -math.sin(theta[2]), 0.0],
                     [math.sin(theta[2]), math.cos(theta[2]), 0.0],
                     [0.0, 0.0, 1.0]])
    # print(rx)
    # print(ry)
    # print(rz)
    # temp = np.matmul(rz,ry)
    # print(temp)

    # r = np.matmul(temp,rx)
    r = rz@ry@rx
    # print(r)
    return r
    # r = np.matmul(np.matmul(rz,ry),rx)




def key_call(x):
    global to_reset,yaw_deg,roll_deg,pitch_deg,trans_init,angel_pixel,distance_pixel
    # a = keyboard.KeyboardEvent('down',28,'enter')
    if x.event_type == 'down' and x.name == 'q':
        yaw_deg = yaw_deg + angel_pixel
        # // 转化为弧度
        roll_arc = roll_deg * DEG_TO_ARC;  # // 绕X轴
        pitch_arc = pitch_deg * DEG_TO_ARC;  # // 绕Y轴
        yaw_arc = yaw_deg * DEG_TO_ARC;  # // 绕Z轴
        print("你按下了 q 键",yaw_deg, roll_deg, pitch_deg)
        print("你按下了 q 键",roll_arc, pitch_arc, yaw_arc)

        rr = eulerAnglesToRotationMatrix([roll_arc, pitch_arc, yaw_arc])
        trans_init[0:3,0:3] = rr
        # print(trans_init)
        pcd_sourse.transform(trans_init)

        # vis.update_geometry(pcd_sourse)
        # if to_reset:
        #     vis.reset_view_point(True)
        #     to_reset = False
        # vis.poll_events()
        # vis.update_renderer()
        # print('to_reset ',to_reset)

        o3d.visualization.draw_geometries([pcd_target, pcd_sourse])



def key_q(vis):
    global yaw_deg,roll_deg,pitch_deg,trans_init,angel_pixel,temp_r
    pcd_sourse.rotate(temp_r)

    yaw_deg = yaw_deg + angel_pixel
    # // 转化为弧度
    roll_arc = roll_deg * DEG_TO_ARC;  # // 绕X轴
    pitch_arc = pitch_deg * DEG_TO_ARC;  # // 绕Y轴
    yaw_arc = yaw_deg * DEG_TO_ARC;  # // 绕Z轴
    print("你按下了 q 键",yaw_deg, roll_deg, pitch_deg)

    R1 = pcd_sourse.get_rotation_matrix_from_xyz((roll_arc, pitch_arc, yaw_arc))
    # r2 = o3d.geometry.get_rotation_matrix_from_xyz((roll_arc, pitch_arc, yaw_arc))
    pcd_sourse.rotate(R1)  # 不指定旋转中心
    print("旋转矩阵：\n", R1)
    vis.update_geometry(pcd_sourse)
    # R1 = pcd_sourse.get_rotation_matrix_from_xyz((-roll_arc, -pitch_arc, -yaw_arc))
    R1 = np.linalg.inv(R1)
    temp_r = R1


def key_a(vis):
    global yaw_deg,roll_deg,pitch_deg,trans_init,angel_pixel,temp_r
    pcd_sourse.rotate(temp_r)
    yaw_deg = yaw_deg - angel_pixel
    # // 转化为弧度
    roll_arc = roll_deg * DEG_TO_ARC;  # // 绕X轴
    pitch_arc = pitch_deg * DEG_TO_ARC;  # // 绕Y轴
    yaw_arc = yaw_deg * DEG_TO_ARC;  # // 绕Z轴
    print("你按下了 a 键",yaw_deg, roll_deg, pitch_deg)
    # print("你按下了 q 键",roll_arc, pitch_arc, yaw_arc)

    R1 = pcd_sourse.get_rotation_matrix_from_xyz((roll_arc, pitch_arc, yaw_arc))
    pcd_sourse.rotate(R1)  # 不指定旋转中心
    print("旋转矩阵：\n", R1)

    vis.update_geometry(pcd_sourse)
    # R1 = pcd_sourse.get_rotation_matrix_from_zyx((-roll_arc, -pitch_arc, -yaw_arc))
    R1 = np.linalg.inv(R1)
    temp_r = R1



def key_w(vis):
    global yaw_deg,roll_deg,pitch_deg,trans_init,angel_pixel,temp_r
    pcd_sourse.rotate(temp_r)

    roll_deg = roll_deg + angel_pixel
    # // 转化为弧度
    roll_arc = roll_deg * DEG_TO_ARC;  # // 绕X轴
    pitch_arc = pitch_deg * DEG_TO_ARC;  # // 绕Y轴
    yaw_arc = yaw_deg * DEG_TO_ARC;  # // 绕Z轴
    print("你按下了 w 键",yaw_deg, roll_deg, pitch_deg)
    # print("你按下了 q 键",roll_arc, pitch_arc, yaw_arc)

    R1 = pcd_sourse.get_rotation_matrix_from_xyz((roll_arc, pitch_arc, yaw_arc))
    pcd_sourse.rotate(R1)  # 不指定旋转中心
    print("旋转矩阵：\n", R1)
    vis.update_geometry(pcd_sourse)
    # R1 = pcd_sourse.get_rotation_matrix_from_zyx((-roll_arc, -pitch_arc, -yaw_arc))
    R1 = np.linalg.inv(R1)
    temp_r = R1


def key_s(vis):
    global yaw_deg,roll_deg,pitch_deg,trans_init,angel_pixel,temp_r
    pcd_sourse.rotate(temp_r)

    roll_deg = roll_deg - angel_pixel
    # // 转化为弧度
    roll_arc = roll_deg * DEG_TO_ARC;  # // 绕X轴
    pitch_arc = pitch_deg * DEG_TO_ARC;  # // 绕Y轴
    yaw_arc = yaw_deg * DEG_TO_ARC;  # // 绕Z轴
    print("你按下了 s 键",yaw_deg, roll_deg, pitch_deg)
    # print("你按下了 q 键",roll_arc, pitch_arc, yaw_arc)

    R1 = pcd_sourse.get_rotation_matrix_from_xyz((roll_arc, pitch_arc, yaw_arc))
    pcd_sourse.rotate(R1)  # 不指定旋转中心
    print("旋转矩阵：\n", R1)
    vis.update_geometry(pcd_sourse)
    # R1 = pcd_sourse.get_rotation_matrix_from_zyx((-roll_arc, -pitch_arc, -yaw_arc))
    R1 = np.linalg.inv(R1)
    temp_r = R1


def key_e(vis):
    global yaw_deg,roll_deg,pitch_deg,trans_init,angel_pixel,temp_r
    pcd_sourse.rotate(temp_r)

    pitch_deg = pitch_deg + angel_pixel
    # // 转化为弧度
    roll_arc = roll_deg * DEG_TO_ARC;  # // 绕X轴
    pitch_arc = pitch_deg * DEG_TO_ARC;  # // 绕Y轴
    yaw_arc = yaw_deg * DEG_TO_ARC;  # // 绕Z轴
    print("你按下了 e 键",yaw_deg, roll_deg, pitch_deg)
    # print("你按下了 q 键",roll_arc, pitch_arc, yaw_arc)

    R1 = pcd_sourse.get_rotation_matrix_from_xyz((roll_arc, pitch_arc, yaw_arc))
    pcd_sourse.rotate(R1)  # 不指定旋转中心
    print("旋转矩阵：\n", R1)
    vis.update_geometry(pcd_sourse)
    # R1 = pcd_sourse.get_rotation_matrix_from_zyx((-roll_arc, -pitch_arc, -yaw_arc))
    R1 = np.linalg.inv(R1)
    temp_r = R1

def key_d(vis):
    global yaw_deg,roll_deg,pitch_deg,trans_init,angel_pixel,temp_r
    pcd_sourse.rotate(temp_r)

    pitch_deg = pitch_deg - angel_pixel
    # // 转化为弧度
    roll_arc = roll_deg * DEG_TO_ARC;  # // 绕X轴
    pitch_arc = pitch_deg * DEG_TO_ARC;  # // 绕Y轴
    yaw_arc = yaw_deg * DEG_TO_ARC;  # // 绕Z轴
    print("你按下了 d 键",yaw_deg, roll_deg, pitch_deg)

    R1 = pcd_sourse.get_rotation_matrix_from_xyz((roll_arc, pitch_arc, yaw_arc))
    pcd_sourse.rotate(R1)  # 不指定旋转中心
    print("旋转矩阵：\n", R1)
    vis.update_geometry(pcd_sourse)
    # R1 = pcd_sourse.get_rotation_matrix_from_zyx((-roll_arc, -pitch_arc, -yaw_arc))
    # pcd_sourse.rotate(R1)  # 不指定旋转中心
    R1 = np.linalg.inv(R1)
    temp_r = R1





# xyz move
def key_r(vis):
    global xd,yd,zd,trans_init,temp_t,distance_pixel
    # pcd_sourse.transform(trans_init)
    pcd_sourse.translate((temp_t[0], temp_t[1], temp_t[2]))

    xd = xd + distance_pixel
    print("你按下了 r 键; xyz偏移为 ", xd,yd,zd)

    pcd_sourse.translate((xd, yd, zd))
    vis.update_geometry(pcd_sourse)
    temp_t[0] = -xd
    temp_t[1] = -yd
    temp_t[2] = -zd

def key_f(vis):
    global xd,yd,zd,trans_init,temp_t,distance_pixel
    pcd_sourse.translate((temp_t[0], temp_t[1], temp_t[2]))

    xd = xd - distance_pixel
    print("你按下了 f 键; xyz偏移为 ", xd,yd,zd)
    pcd_sourse.translate((xd, yd, zd))
    vis.update_geometry(pcd_sourse)
    temp_t[0] = -xd
    temp_t[1] = -yd
    temp_t[2] = -zd


def key_t(vis):
    global xd,yd,zd,trans_init,temp_t,distance_pixel
    pcd_sourse.translate((temp_t[0], temp_t[1], temp_t[2]))

    yd = yd + distance_pixel
    print("你按下了 t 键; xyz偏移为 ", xd,yd,zd)

    pcd_sourse.translate((xd, yd, zd))
    vis.update_geometry(pcd_sourse)
    temp_t[0] = -xd
    temp_t[1] = -yd
    temp_t[2] = -zd

def key_g(vis):
    global xd,yd,zd,trans_init,temp_t,distance_pixel
    pcd_sourse.translate((temp_t[0], temp_t[1], temp_t[2]))

    yd = yd - distance_pixel
    print("你按下了 g 键; xyz偏移为 ", xd,yd,zd)

    pcd_sourse.translate((xd, yd, zd))
    vis.update_geometry(pcd_sourse)
    temp_t[0] = -xd
    temp_t[1] = -yd
    temp_t[2] = -zd

def key_y(vis):
    global xd,yd,zd,trans_init,temp_t,distance_pixel
    pcd_sourse.translate((temp_t[0], temp_t[1], temp_t[2]))

    zd = zd + distance_pixel
    print("你按下了 y 键; xyz偏移为 ", xd,yd,zd)

    pcd_sourse.translate((xd, yd, zd))
    vis.update_geometry(pcd_sourse)
    temp_t[0] = -xd
    temp_t[1] = -yd
    temp_t[2] = -zd


def key_h(vis):
    global xd,yd,zd,trans_init,temp_t,distance_pixel
    pcd_sourse.translate((temp_t[0], temp_t[1], temp_t[2]))

    zd = zd - distance_pixel
    print("你按下了 h 键; xyz偏移为 ", xd,yd,zd)

    pcd_sourse.translate((xd, yd, zd))
    vis.update_geometry(pcd_sourse)
    temp_t[0] = -xd
    temp_t[1] = -yd
    temp_t[2] = -zd



def filter_dis(cloud):
    tp = np.asarray(cloud.points)
    dis = tp[:, 0]*tp[:, 0] + tp[:, 1]*tp[:, 1] + tp[:, 2]*tp[:, 2]
    new_data_c = tp[dis >= 5*5]

    # 高度过滤
    z = new_data_c[:, 2]
    new_data_c = new_data_c[z >= -0.6]

    # new_cloud = o3d.geometry.PointCloud()
    # new_cloud.points = o3d.utility.Vector3dVector(new_data_c)
    cloud.points = o3d.utility.Vector3dVector(new_data_c)
    # return new_cloud




if __name__ == '__main__':



    trans_init = np.asarray([[1.0, 0.0, 0.0, 0.0],
                             [0.0, 1.0, 0.0, 0.0],
                             [0.0, 0.0, 1.0, 0.0],
                             [0.0, 0.0, 0.0, 1.0]]).astype(np.float64)

    temp_r = np.asarray([[1.0, 0.0, 0.0],
                             [0.0, 1.0, 0.0],
                             [0.0, 0.0, 1.0]]).astype(np.float64)
    temp_t = np.asarray([0.0, 0.0, 0.0]).astype(np.float64)

    # trans_init = np.asarray([
    # [-6.97562996e-02 , 9.97564062e-01  ,1.74532925e-07, 3.6-1.54],
    # [-9.97564062e-01 ,- 6.97562996e-02 ,- 1.74532925e-07, -5.04999+2.60],
    # [-1.61933003e-07 ,- 1.86282545e-07 , 1.00000000e+00, 0 ],
    #                          [0.0, 0.0, 0.0, 1.0]]).astype(np.float64)


    # 分辨率
    angel_pixel = 1.
    distance_pixel = 0.05

    path1 = r"E:\cpp_project\lidar_app_calib\pcldemo\lidar1.pcd"
    path2 = r"E:\cpp_project\lidar_app_calib\pcldemo\lidar2.pcd"

    pcd_target = o3d.io.read_point_cloud(path1)
    pcd_sourse = o3d.io.read_point_cloud(path2)


    pcd_target.paint_uniform_color([0, 1, 0])
    pcd_sourse.paint_uniform_color([1, 0, 0])
    pcd_sourse.transform(trans_init)



    vis = o3d.visualization.Visualizer()

    ARC_TO_DEG = 57.29577951308238;
    DEG_TO_ARC = 0.0174532925199433;
    # // 设定车体欧拉角(角度)，绕固定轴
    roll_deg = 0.00001;  # // 绕X轴
    pitch_deg = 0.00001;  # // 绕Y轴
    yaw_deg = 0.00001;  # // 绕Z轴
    xd = 0.000001;
    yd = 0.000001;
    zd = 0.000001;

    key_to_callback = {}
    key_to_callback[ord("Q")] = key_q
    key_to_callback[ord("A")] = key_a
    key_to_callback[ord("W")] = key_w
    key_to_callback[ord("S")] = key_s
    key_to_callback[ord("E")] = key_e
    key_to_callback[ord("D")] = key_d

    key_to_callback[ord("R")] = key_r
    key_to_callback[ord("F")] = key_f
    key_to_callback[ord("T")] = key_t
    key_to_callback[ord("G")] = key_g
    key_to_callback[ord("Y")] = key_y
    key_to_callback[ord("H")] = key_h


    o3d.visualization.draw_geometries_with_key_callbacks([pcd_target, pcd_sourse], key_to_callback)