一、总转换关系liar–base_link–odom–map二、雷达转base_link2.1 雷达坐标下的点云图由于机器狗的安装角度引入了roll和pitch的转动需要旋转讲roll和pitch旋转后会与X-Y面平行转到了odom坐标系。我这是lio-livox官方算法所以建立地图的地点就是odom坐标系Tx0.187;Tz0.0803;roll-0.3;pitch12.73;记住是左乘p_body T_body_lidar p_lidar三、base_link转odom由于机器狗建立起点的摆放位置会引入Z轴的旋转yaw8.1度这里是讲点云拉正不是坐标系转换需要取反yaw-8.1p_odom T_odom_baselink p_baselink四、odom转mapTx1.36;Ty2.8;Tz0.22;可以看到已经变到了墙角五。常用小工具计算4*4转换矩阵#!/usr/bin/env python3 #-*-coding:utf-8-*-import math import numpy as np defmake_transform(tx,ty,tz,roll_deg,pitch_deg,yaw_deg): 根据 TX,TY,TZ,ROLL,PITCH,YAW 生成4x4 齐次变换矩阵。 平移单位米 角度单位度 欧拉角顺序 RRz(yaw)Ry(pitch)Rx(roll)即 roll:绕 X 轴旋转 pitch:绕 Y 轴旋转 yaw:绕 Z 轴旋转 rollmath.radians(roll_deg)pitchmath.radians(pitch_deg)yawmath.radians(yaw_deg)crmath.cos(roll)srmath.sin(roll)cpmath.cos(pitch)spmath.sin(pitch)cymath.cos(yaw)symath.sin(yaw)Rxnp.array([[1.0,0.0,0.0],[0.0,cr,-sr],[0.0,sr,cr]],dtypenp.float64)Rynp.array([[cp,0.0,sp],[0.0,1.0,0.0],[-sp,0.0,cp]],dtypenp.float64)Rznp.array([[cy,-sy,0.0],[sy,cy,0.0],[0.0,0.0,1.0]],dtypenp.float64)RRz Ry Rx Tnp.eye(4,dtypenp.float64)T[:3,:3]R T[0,3]tx T[1,3]ty T[2,3]tzreturnT defprint_matrix(T): 按适合保存为 txt 的格式输出矩阵。fori inrange(4):print({:.10f} {:.10f} {:.10f} {:.10f}.format(T[i,0],T[i,1],T[i,2],T[i,3]))if__name____main__:## 你只需要改这里 #TX0TY0.0TZ0ROLL_DEG0PITCH_DEG0YAW_DEG-7.8#Tmake_transform(txTX,tyTY,tzTZ,roll_degROLL_DEG,pitch_degPITCH_DEG,yaw_degYAW_DEG)print(4x4 transform matrix:)print_matrix(T)计算转换点云讲原始点云左乘矩阵#!/usr/bin/env python3 #-*-coding:utf-8-*-import os import numpy as np PCD_INPUTodom-2/mid360_lio_livox_odom.pcdMATRIX_TXTodom_map-2/level_transform_matrix_odom_map.txtPCD_OUTPUTodom_map-2/mid360_lio_livox_map.pcddefread_pcd_binary_float32(path): 读取 binary PCD。 当前版本适用于你的 PCD 格式 FIELDS x y z intensity SIZE4444TYPE F F F F DATA binary withopen(path,rb)as f:header_lines[]whileTrue:linef.readline()ifnot line:raiseRuntimeError(没有找到 DATA 行PCD 格式异常。)header_lines.append(line)ifline.startswith(bDATA):breakdataf.read()headerb.join(header_lines)header_textheader.decode(ascii,errorsignore)fieldsNone sizeNone typNone countNoneforline in header_text.splitlines():partsline.split()ifnot parts:continueifparts[0]FIELDS:fieldsparts[1:]elif parts[0]SIZE:sizelist(map(int,parts[1:]))elif parts[0]TYPE:typparts[1:]elif parts[0]COUNT:countlist(map(int,parts[1:]))iffields is None:raiseRuntimeError(PCD 中没有 FIELDS。)ifxnot in fields orynot in fields orznot in fields:raiseRuntimeError(PCD 中必须包含 x y z 字段。)ifsize![4]*len(fields)or typ![F]*len(fields)or count![1]*len(fields):raiseRuntimeError(当前程序只支持 float32 scalar binary PCD例如 FIELDS x y z intensity。)arrnp.frombuffer(data,dtypenp.float32).copy()arrarr.reshape((-1,len(fields)))returnheader,fields,arr defwrite_pcd_binary_float32(path,header,arr):withopen(path,wb)as f:f.write(header)f.write(arr.astype(np.float32).tobytes())deftransform_lidar_pcd_to_body(input_pcd,matrix_txt,output_pcd):ifnot os.path.exists(input_pcd):raiseFileNotFoundError(找不到输入 PCD: {}.format(input_pcd))ifnot os.path.exists(matrix_txt):raiseFileNotFoundError(找不到矩阵 txt: {}.format(matrix_txt))T_body_lidarnp.loadtxt(matrix_txt,dtypenp.float64)ifT_body_lidar.shape!(4,4):raiseRuntimeError(矩阵必须是 4x4。)print([INFO] T_body_lidar:)np.set_printoptions(precision10,suppressTrue)print(T_body_lidar)header,fields,arrread_pcd_binary_float32(input_pcd)ixfields.index(x)iyfields.index(y)izfields.index(z)xyz_lidararr[:,[ix,iy,iz]].astype(np.float64)print([INFO] points:,xyz_lidar.shape[0])print([INFO] lidar XYZ range:)print( x: {:.3f} ~ {:.3f}.format(xyz_lidar[:,0].min(),xyz_lidar[:,0].max()))print( y: {:.3f} ~ {:.3f}.format(xyz_lidar[:,1].min(),xyz_lidar[:,1].max()))print( z: {:.3f} ~ {:.3f}.format(xyz_lidar[:,2].min(),xyz_lidar[:,2].max()))# 齐次坐标[x,y,z,1]^T xyz_hnp.ones((xyz_lidar.shape[0],4),dtypenp.float64)xyz_h[:,0:3]xyz_lidar # 关键左乘#p_bodyT_body_lidar p_lidarxyz_body_h(T_body_lidar xyz_h.T).T xyz_bodyxyz_body_h[:,0:3]arr[:,[ix,iy,iz]]xyz_body.astype(np.float32)write_pcd_binary_float32(output_pcd,header,arr)print([INFO] body XYZ range:)print( x: {:.3f} ~ {:.3f}.format(xyz_body[:,0].min(),xyz_body[:,0].max()))print( y: {:.3f} ~ {:.3f}.format(xyz_body[:,1].min(),xyz_body[:,1].max()))print( z: {:.3f} ~ {:.3f}.format(xyz_body[:,2].min(),xyz_body[:,2].max()))print([INFO] saved:,output_pcd)if__name____main__:transform_lidar_pcd_to_body(input_pcdPCD_INPUT,matrix_txtMATRIX_TXT,output_pcdPCD_OUTPUT)画图画3个平面的投影#!/usr/bin/env python3 #-*-coding:utf-8-*-import os import numpy as np import open3d as o3d import matplotlib.pyplot as plt ## 这里改成你的文件路径 #PCD_PATHodom_map-2/mid360_lio_livox_map.pcd# 输出图片路径 OUT_XYodom_map-2/pcd_xy_projection.pngOUT_XZodom_map-2/pcd_xz_projection.pngOUT_YZodom_map-2/pcd_yz_projection.png# 是否按 z/y/x 着色只是显示更直观不改变数据 COLOR_BY_VALUEFalse # 点大小 POINT_SIZE0.3defload_pcd(pcd_path):ifnot os.path.exists(pcd_path):raiseFileNotFoundError(fPCD 文件不存在: {pcd_path})print(f[INFO] Loading PCD: {pcd_path})pcdo3d.io.read_point_cloud(pcd_path)ifpcd.is_empty():raiseRuntimeError(PCD 文件为空。)pointsnp.asarray(pcd.points,dtypenp.float64)# 去除无效值 masknp.isfinite(points).all(axis1)pointspoints[mask]ifpoints.shape[0]0:raiseRuntimeError(点云中没有有效点。)print(f[INFO] Point number: {points.shape[0]})print(f[INFO] X range: {points[:,0].min():.3f} ~ {points[:,0].max():.3f})print(f[INFO] Y range: {points[:,1].min():.3f} ~ {points[:,1].max():.3f})print(f[INFO] Z range: {points[:,2].min():.3f} ~ {points[:,2].max():.3f})returnpoints defplot_projection(x,y,xlabel,ylabel,title,save_path,color_valuesNone):plt.figure(figsize(10,10))ifcolor_values is not None:scplt.scatter(x,y,ccolor_values,sPOINT_SIZE)cbarplt.colorbar(sc)cbar.set_label(value)else:plt.scatter(x,y,sPOINT_SIZE)plt.xlabel(xlabel)plt.ylabel(ylabel)plt.title(title)# 保持坐标比例一致避免投影被拉伸 plt.axis(equal)plt.grid(True)plt.tight_layout()plt.savefig(save_path,dpi300)print(f[INFO] Saved: {save_path})plt.show()defmain():pointsload_pcd(PCD_PATH)xpoints[:,0]ypoints[:,1]zpoints[:,2]#1)XY 投影ifCOLOR_BY_VALUE:plot_projection(x,y,xlabelX (m),ylabelY (m),titlePCD Projection on X-Y Plane,save_pathOUT_XY,color_valuesz)else:plot_projection(x,y,xlabelX (m),ylabelY (m),titlePCD Projection on X-Y Plane,save_pathOUT_XY)#2)XZ 投影ifCOLOR_BY_VALUE:plot_projection(x,z,xlabelX (m),ylabelZ (m),titlePCD Projection on X-Z Plane,save_pathOUT_XZ,color_valuesy)else:plot_projection(x,z,xlabelX (m),ylabelZ (m),titlePCD Projection on X-Z Plane,save_pathOUT_XZ)#3)YZ 投影ifCOLOR_BY_VALUE:plot_projection(y,z,xlabelY (m),ylabelZ (m),titlePCD Projection on Y-Z Plane,save_pathOUT_YZ,color_valuesx)else:plot_projection(y,z,xlabelY (m),ylabelZ (m),titlePCD Projection on Y-Z Plane,save_pathOUT_YZ)if__name____main__:main()