devbox
凡人多烦事01
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

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

作者：凡人多烦事01 | 2024-04-14 07:46:53

赞

踩

python 外参校准算法

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

 

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

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

 

 

标定流程为：

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

 

 

 

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

# q,a  偏航角调整

# w,s  roll 翻滚角调整

# e,d  pitch 俯仰角调整

# r,f    X 轴调整

# t,g    Y 轴调整

# y,h    Z 轴调整

 

a05e42919f1347ae8cc2ce96f3593062.png

原始未标定数据

 

 

ac116ce2612d43dfa5bcfec3274a84ae.png

 

标定后的数据

 

 

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

 

  1. # coding:utf-8
  2.  
  3. import open3d as o3d
  4. import numpy as np
  5. import math
  6. import copy
  7.  
  8. # 第一版本使用角度转换的旋转矩阵，
  9. # 然后把每次的旋转角度加起来，累计；
  10. # 或者把每次的旋转矩阵累乘，都不是最后的正确结果，很奇怪
  11. # 第二版本改为使用矩阵的逆矩阵，没问题了，应该是矩阵的旋转顺序有影响
  12.  
  13.  
  14.  
  15. # 通过以下按键来调整点云位姿:
  16. # q,a 偏航角调整
  17. # w,s roll 翻滚角调整
  18. # e,d pitch 俯仰角调整
  19. # r,f  X 轴调整
  20. # t,g  Y 轴调整
  21. # y,h  Z 轴调整
  22.  
  23.  
  24. def eulerAnglesToRotationMatrix(theta):
  25.     rx = np.asarray([[1.0, 0.0, 0.0],
  26.                      [0.0, math.cos(theta[0]), -math.sin(theta[0])],
  27.                      [0.0, math.sin(theta[0]), math.cos(theta[0])]])
  28.  
  29.     ry = np.asarray([[math.cos(theta[1]), 0.0, math.sin(theta[1])],
  30.                      [0.0, 1.0, 0.0],
  31.                      [-math.sin(theta[1]), 0.0, math.cos(theta[1])]])
  32.  
  33.     rz = np.asarray([[math.cos(theta[2]), -math.sin(theta[2]), 0.0],
  34.                      [math.sin(theta[2]), math.cos(theta[2]), 0.0],
  35.                      [0.0, 0.0, 1.0]])
  36.     # print(rx)
  37.     # print(ry)
  38.     # print(rz)
  39.     # temp = np.matmul(rz,ry)
  40.     # print(temp)
  41.  
  42.     # r = np.matmul(temp,rx)
  43.     r = rz@ry@rx
  44.     # print(r)
  45.     return r
  46.     # r = np.matmul(np.matmul(rz,ry),rx)
  47.  
  48.  
  49.  
  50.  
  51. def key_call(x):
  52.     global to_reset,yaw_deg,roll_deg,pitch_deg,trans_init,angel_pixel,distance_pixel
  53.     # a = keyboard.KeyboardEvent('down',28,'enter')
  54.     if x.event_type == 'down' and x.name == 'q':
  55.         yaw_deg = yaw_deg + angel_pixel
  56.         # // 转化为弧度
  57.         roll_arc = roll_deg * DEG_TO_ARC;  # // 绕X轴
  58.         pitch_arc = pitch_deg * DEG_TO_ARC;  # // 绕Y轴
  59.         yaw_arc = yaw_deg * DEG_TO_ARC;  # // 绕Z轴
  60.         print("你按下了 q 键",yaw_deg, roll_deg, pitch_deg)
  61.         print("你按下了 q 键",roll_arc, pitch_arc, yaw_arc)
  62.  
  63.         rr = eulerAnglesToRotationMatrix([roll_arc, pitch_arc, yaw_arc])
  64.         trans_init[0:3,0:3] = rr
  65.         # print(trans_init)
  66.         pcd_sourse.transform(trans_init)
  67.  
  68.         # vis.update_geometry(pcd_sourse)
  69.         # if to_reset:
  70.         #     vis.reset_view_point(True)
  71.         #     to_reset = False
  72.         # vis.poll_events()
  73.         # vis.update_renderer()
  74.         # print('to_reset ',to_reset)
  75.  
  76.         o3d.visualization.draw_geometries([pcd_target, pcd_sourse])
  77.  
  78.  
  79.  
  80. def key_q(vis):
  81.     global yaw_deg,roll_deg,pitch_deg,trans_init,angel_pixel,temp_r
  82.     pcd_sourse.rotate(temp_r)
  83.  
  84.     yaw_deg = yaw_deg + angel_pixel
  85.     # // 转化为弧度
  86.     roll_arc = roll_deg * DEG_TO_ARC;  # // 绕X轴
  87.     pitch_arc = pitch_deg * DEG_TO_ARC;  # // 绕Y轴
  88.     yaw_arc = yaw_deg * DEG_TO_ARC;  # // 绕Z轴
  89.     print("你按下了 q 键",yaw_deg, roll_deg, pitch_deg)
  90.  
  91.     R1 = pcd_sourse.get_rotation_matrix_from_xyz((roll_arc, pitch_arc, yaw_arc))
  92.     # r2 = o3d.geometry.get_rotation_matrix_from_xyz((roll_arc, pitch_arc, yaw_arc))
  93.     pcd_sourse.rotate(R1)  # 不指定旋转中心
  94.     print("旋转矩阵：\n", R1)
  95.     vis.update_geometry(pcd_sourse)
  96.     # R1 = pcd_sourse.get_rotation_matrix_from_xyz((-roll_arc, -pitch_arc, -yaw_arc))
  97.     R1 = np.linalg.inv(R1)
  98.     temp_r = R1
  99.  
  100.  
  101. def key_a(vis):
  102.     global yaw_deg,roll_deg,pitch_deg,trans_init,angel_pixel,temp_r
  103.     pcd_sourse.rotate(temp_r)
  104.     yaw_deg = yaw_deg - angel_pixel
  105.     # // 转化为弧度
  106.     roll_arc = roll_deg * DEG_TO_ARC;  # // 绕X轴
  107.     pitch_arc = pitch_deg * DEG_TO_ARC;  # // 绕Y轴
  108.     yaw_arc = yaw_deg * DEG_TO_ARC;  # // 绕Z轴
  109.     print("你按下了 a 键",yaw_deg, roll_deg, pitch_deg)
  110.     # print("你按下了 q 键",roll_arc, pitch_arc, yaw_arc)
  111.  
  112.     R1 = pcd_sourse.get_rotation_matrix_from_xyz((roll_arc, pitch_arc, yaw_arc))
  113.     pcd_sourse.rotate(R1)  # 不指定旋转中心
  114.     print("旋转矩阵：\n", R1)
  115.  
  116.     vis.update_geometry(pcd_sourse)
  117.     # R1 = pcd_sourse.get_rotation_matrix_from_zyx((-roll_arc, -pitch_arc, -yaw_arc))
  118.     R1 = np.linalg.inv(R1)
  119.     temp_r = R1
  120.  
  121.  
  122.  
  123. def key_w(vis):
  124.     global yaw_deg,roll_deg,pitch_deg,trans_init,angel_pixel,temp_r
  125.     pcd_sourse.rotate(temp_r)
  126.  
  127.     roll_deg = roll_deg + angel_pixel
  128.     # // 转化为弧度
  129.     roll_arc = roll_deg * DEG_TO_ARC;  # // 绕X轴
  130.     pitch_arc = pitch_deg * DEG_TO_ARC;  # // 绕Y轴
  131.     yaw_arc = yaw_deg * DEG_TO_ARC;  # // 绕Z轴
  132.     print("你按下了 w 键",yaw_deg, roll_deg, pitch_deg)
  133.     # print("你按下了 q 键",roll_arc, pitch_arc, yaw_arc)
  134.  
  135.     R1 = pcd_sourse.get_rotation_matrix_from_xyz((roll_arc, pitch_arc, yaw_arc))
  136.     pcd_sourse.rotate(R1)  # 不指定旋转中心
  137.     print("旋转矩阵：\n", R1)
  138.     vis.update_geometry(pcd_sourse)
  139.     # R1 = pcd_sourse.get_rotation_matrix_from_zyx((-roll_arc, -pitch_arc, -yaw_arc))
  140.     R1 = np.linalg.inv(R1)
  141.     temp_r = R1
  142.  
  143.  
  144. def key_s(vis):
  145.     global yaw_deg,roll_deg,pitch_deg,trans_init,angel_pixel,temp_r
  146.     pcd_sourse.rotate(temp_r)
  147.  
  148.     roll_deg = roll_deg - angel_pixel
  149.     # // 转化为弧度
  150.     roll_arc = roll_deg * DEG_TO_ARC;  # // 绕X轴
  151.     pitch_arc = pitch_deg * DEG_TO_ARC;  # // 绕Y轴
  152.     yaw_arc = yaw_deg * DEG_TO_ARC;  # // 绕Z轴
  153.     print("你按下了 s 键",yaw_deg, roll_deg, pitch_deg)
  154.     # print("你按下了 q 键",roll_arc, pitch_arc, yaw_arc)
  155.  
  156.     R1 = pcd_sourse.get_rotation_matrix_from_xyz((roll_arc, pitch_arc, yaw_arc))
  157.     pcd_sourse.rotate(R1)  # 不指定旋转中心
  158.     print("旋转矩阵：\n", R1)
  159.     vis.update_geometry(pcd_sourse)
  160.     # R1 = pcd_sourse.get_rotation_matrix_from_zyx((-roll_arc, -pitch_arc, -yaw_arc))
  161.     R1 = np.linalg.inv(R1)
  162.     temp_r = R1
  163.  
  164.  
  165. def key_e(vis):
  166.     global yaw_deg,roll_deg,pitch_deg,trans_init,angel_pixel,temp_r
  167.     pcd_sourse.rotate(temp_r)
  168.  
  169.     pitch_deg = pitch_deg + angel_pixel
  170.     # // 转化为弧度
  171.     roll_arc = roll_deg * DEG_TO_ARC;  # // 绕X轴
  172.     pitch_arc = pitch_deg * DEG_TO_ARC;  # // 绕Y轴
  173.     yaw_arc = yaw_deg * DEG_TO_ARC;  # // 绕Z轴
  174.     print("你按下了 e 键",yaw_deg, roll_deg, pitch_deg)
  175.     # print("你按下了 q 键",roll_arc, pitch_arc, yaw_arc)
  176.  
  177.     R1 = pcd_sourse.get_rotation_matrix_from_xyz((roll_arc, pitch_arc, yaw_arc))
  178.     pcd_sourse.rotate(R1)  # 不指定旋转中心
  179.     print("旋转矩阵：\n", R1)
  180.     vis.update_geometry(pcd_sourse)
  181.     # R1 = pcd_sourse.get_rotation_matrix_from_zyx((-roll_arc, -pitch_arc, -yaw_arc))
  182.     R1 = np.linalg.inv(R1)
  183.     temp_r = R1
  184.  
  185. def key_d(vis):
  186.     global yaw_deg,roll_deg,pitch_deg,trans_init,angel_pixel,temp_r
  187.     pcd_sourse.rotate(temp_r)
  188.  
  189.     pitch_deg = pitch_deg - angel_pixel
  190.     # // 转化为弧度
  191.     roll_arc = roll_deg * DEG_TO_ARC;  # // 绕X轴
  192.     pitch_arc = pitch_deg * DEG_TO_ARC;  # // 绕Y轴
  193.     yaw_arc = yaw_deg * DEG_TO_ARC;  # // 绕Z轴
  194.     print("你按下了 d 键",yaw_deg, roll_deg, pitch_deg)
  195.  
  196.     R1 = pcd_sourse.get_rotation_matrix_from_xyz((roll_arc, pitch_arc, yaw_arc))
  197.     pcd_sourse.rotate(R1)  # 不指定旋转中心
  198.     print("旋转矩阵：\n", R1)
  199.     vis.update_geometry(pcd_sourse)
  200.     # R1 = pcd_sourse.get_rotation_matrix_from_zyx((-roll_arc, -pitch_arc, -yaw_arc))
  201.     # pcd_sourse.rotate(R1)  # 不指定旋转中心
  202.     R1 = np.linalg.inv(R1)
  203.     temp_r = R1
  204.  
  205.  
  206.  
  207.  
  208.  
  209. # xyz move
  210. def key_r(vis):
  211.     global xd,yd,zd,trans_init,temp_t,distance_pixel
  212.     # pcd_sourse.transform(trans_init)
  213.     pcd_sourse.translate((temp_t[0], temp_t[1], temp_t[2]))
  214.  
  215.     xd = xd + distance_pixel
  216.     print("你按下了 r 键; xyz偏移为 ", xd,yd,zd)
  217.  
  218.     pcd_sourse.translate((xd, yd, zd))
  219.     vis.update_geometry(pcd_sourse)
  220.     temp_t[0] = -xd
  221.     temp_t[1] = -yd
  222.     temp_t[2] = -zd
  223.  
  224. def key_f(vis):
  225.     global xd,yd,zd,trans_init,temp_t,distance_pixel
  226.     pcd_sourse.translate((temp_t[0], temp_t[1], temp_t[2]))
  227.  
  228.     xd = xd - distance_pixel
  229.     print("你按下了 f 键; xyz偏移为 ", xd,yd,zd)
  230.     pcd_sourse.translate((xd, yd, zd))
  231.     vis.update_geometry(pcd_sourse)
  232.     temp_t[0] = -xd
  233.     temp_t[1] = -yd
  234.     temp_t[2] = -zd
  235.  
  236.  
  237. def key_t(vis):
  238.     global xd,yd,zd,trans_init,temp_t,distance_pixel
  239.     pcd_sourse.translate((temp_t[0], temp_t[1], temp_t[2]))
  240.  
  241.     yd = yd + distance_pixel
  242.     print("你按下了 t 键; xyz偏移为 ", xd,yd,zd)
  243.  
  244.     pcd_sourse.translate((xd, yd, zd))
  245.     vis.update_geometry(pcd_sourse)
  246.     temp_t[0] = -xd
  247.     temp_t[1] = -yd
  248.     temp_t[2] = -zd
  249.  
  250. def key_g(vis):
  251.     global xd,yd,zd,trans_init,temp_t,distance_pixel
  252.     pcd_sourse.translate((temp_t[0], temp_t[1], temp_t[2]))
  253.  
  254.     yd = yd - distance_pixel
  255.     print("你按下了 g 键; xyz偏移为 ", xd,yd,zd)
  256.  
  257.     pcd_sourse.translate((xd, yd, zd))
  258.     vis.update_geometry(pcd_sourse)
  259.     temp_t[0] = -xd
  260.     temp_t[1] = -yd
  261.     temp_t[2] = -zd
  262.  
  263. def key_y(vis):
  264.     global xd,yd,zd,trans_init,temp_t,distance_pixel
  265.     pcd_sourse.translate((temp_t[0], temp_t[1], temp_t[2]))
  266.  
  267.     zd = zd + distance_pixel
  268.     print("你按下了 y 键; xyz偏移为 ", xd,yd,zd)
  269.  
  270.     pcd_sourse.translate((xd, yd, zd))
  271.     vis.update_geometry(pcd_sourse)
  272.     temp_t[0] = -xd
  273.     temp_t[1] = -yd
  274.     temp_t[2] = -zd
  275.  
  276.  
  277. def key_h(vis):
  278.     global xd,yd,zd,trans_init,temp_t,distance_pixel
  279.     pcd_sourse.translate((temp_t[0], temp_t[1], temp_t[2]))
  280.  
  281.     zd = zd - distance_pixel
  282.     print("你按下了 h 键; xyz偏移为 ", xd,yd,zd)
  283.  
  284.     pcd_sourse.translate((xd, yd, zd))
  285.     vis.update_geometry(pcd_sourse)
  286.     temp_t[0] = -xd
  287.     temp_t[1] = -yd
  288.     temp_t[2] = -zd
  289.  
  290.  
  291.  
  292. def filter_dis(cloud):
  293.     tp = np.asarray(cloud.points)
  294.     dis = tp[:, 0]*tp[:, 0] + tp[:, 1]*tp[:, 1] + tp[:, 2]*tp[:, 2]
  295.     new_data_c = tp[dis >= 5*5]
  296.  
  297.     # 高度过滤
  298.     z = new_data_c[:, 2]
  299.     new_data_c = new_data_c[z >= -0.6]
  300.  
  301.     # new_cloud = o3d.geometry.PointCloud()
  302.     # new_cloud.points = o3d.utility.Vector3dVector(new_data_c)
  303.     cloud.points = o3d.utility.Vector3dVector(new_data_c)
  304.     # return new_cloud
  305.  
  306.  
  307.  
  308.  
  309. if __name__ == '__main__':
  310.  
  311.  
  312.  
  313.     trans_init = np.asarray([[1.0, 0.0, 0.0, 0.0],
  314.                              [0.0, 1.0, 0.0, 0.0],
  315.                              [0.0, 0.0, 1.0, 0.0],
  316.                              [0.0, 0.0, 0.0, 1.0]]).astype(np.float64)
  317.  
  318.     temp_r = np.asarray([[1.0, 0.0, 0.0],
  319.                              [0.0, 1.0, 0.0],
  320.                              [0.0, 0.0, 1.0]]).astype(np.float64)
  321.     temp_t = np.asarray([0.0, 0.0, 0.0]).astype(np.float64)
  322.  
  323.     # trans_init = np.asarray([
  324.     # [-6.97562996e-02 , 9.97564062e-01  ,1.74532925e-07, 3.6-1.54],
  325.     # [-9.97564062e-01 ,- 6.97562996e-02 ,- 1.74532925e-07, -5.04999+2.60],
  326.     # [-1.61933003e-07 ,- 1.86282545e-07 , 1.00000000e+00, 0 ],
  327.     #                          [0.0, 0.0, 0.0, 1.0]]).astype(np.float64)
  328.  
  329.  
  330.     # 分辨率
  331.     angel_pixel = 1.
  332.     distance_pixel = 0.05
  333.  
  334.     path1 = r"E:\cpp_project\lidar_app_calib\pcldemo\lidar1.pcd"
  335.     path2 = r"E:\cpp_project\lidar_app_calib\pcldemo\lidar2.pcd"
  336.  
  337.     pcd_target = o3d.io.read_point_cloud(path1)
  338.     pcd_sourse = o3d.io.read_point_cloud(path2)
  339.  
  340.  
  341.     pcd_target.paint_uniform_color([0, 1, 0])
  342.     pcd_sourse.paint_uniform_color([1, 0, 0])
  343.     pcd_sourse.transform(trans_init)
  344.  
  345.  
  346.  
  347.     vis = o3d.visualization.Visualizer()
  348.  
  349.     ARC_TO_DEG = 57.29577951308238;
  350.     DEG_TO_ARC = 0.0174532925199433;
  351.     # // 设定车体欧拉角(角度)，绕固定轴
  352.     roll_deg = 0.00001;  # // 绕X轴
  353.     pitch_deg = 0.00001;  # // 绕Y轴
  354.     yaw_deg = 0.00001;  # // 绕Z轴
  355.     xd = 0.000001;
  356.     yd = 0.000001;
  357.     zd = 0.000001;
  358.  
  359.     key_to_callback = {}
  360.     key_to_callback[ord("Q")] = key_q
  361.     key_to_callback[ord("A")] = key_a
  362.     key_to_callback[ord("W")] = key_w
  363.     key_to_callback[ord("S")] = key_s
  364.     key_to_callback[ord("E")] = key_e
  365.     key_to_callback[ord("D")] = key_d
  366.  
  367.     key_to_callback[ord("R")] = key_r
  368.     key_to_callback[ord("F")] = key_f
  369.     key_to_callback[ord("T")] = key_t
  370.     key_to_callback[ord("G")] = key_g
  371.     key_to_callback[ord("Y")] = key_y
  372.     key_to_callback[ord("H")] = key_h
  373.  
  374.  
  375.     o3d.visualization.draw_geometries_with_key_callbacks([pcd_target, pcd_sourse], key_to_callback)
  376.  
  377.  
  378.  
  379.  
  380.  
  381.  
  382.

 

 

 

 

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/凡人多烦事01/article/detail/420951
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号