坐标系统
ROS坐标变换
ROS静态坐标变换和动态坐标变换
tf：TransForm Frame，坐标变换
通过右手坐标系来标定的
1. 静态坐标变换
两个物体之间的坐标变换是 固定的时 采用静态坐标变换
雷达坐标系和车身坐标系
2. 动态坐标变换
两个物体之间的坐标变换是 随时间变换的 采用动态坐标变换
车自身坐标系和世界坐标系相对位置是移动的
geometry_msgs/TransformStamped用于传输 坐标系相关位置信息，
geometry_msgs/PointStamped 用于传输 某个坐标系内坐标点的信息
变化规范
自身坐标系之FLU坐标:X轴向前,Y轴向左,Z轴向上
FLU - 相对于自身。 头朝向为Forword(X)， 左边为Left(Y)， 顶朝向为Up(Z)
ENU - 相对于世界中的东北天(XYZ)
世界坐标系之ENU坐标：X轴向东，Y轴向北，Z轴向上
世界坐标系之NWU坐标：X轴向北，Y轴向西，Z轴向上（这个坐标系最符合人类想象）
TX_I_V I - imu
TX_S_V S - sensor <其他传感器 毫米波雷达等>
TX_L_S L - lidar
TX_C_S C - camera
TX_L_C Transformation from LiDAR to Camera frame
TX_Li_L the i-th lidar in LiDAR frame
TX_Ci_C
L S C 是虚拟的，是同一个点
Li 实体激光雷达
Ci 实体相机
图像的标定参数
内参： camera_matrix 是3x3的内参矩阵
畸变系数： distortion_coefficients 是５个畸变纠正参数
外参：变换矩阵= 平移矩阵 加旋转矩阵(四元数)
Intrinsics & Extrinsics
从相机坐标系转换到像素坐标系中，相机内参的作用
从世界坐标系转换到相机坐标系中，相机外参的作用
欧拉角 姿态矩阵可以通过四元数和欧拉角算出
位姿pose--位姿信息包含位置坐标和角度 (四元数)
一个四元素有4个成员（x,y,z,w）。其中，对于不绕x / y / z轴旋转的 常用单位 四元数为（0,0,0,1）
"pose_orientation": {
"x": -0.0005576330351997091,
"y": -0.0009195997450303073,
"z": -0.9290865615830163,
"w": 0.3698607906598847,
"pose_x": 6855.053677575546,
"pose_y": -19706.759414176922,
"pose_z": 13.437930198386312}
{"pose_matrix": [
-0.03224906846459613, 0.999477679252545, -0.0020895595499665115, 475.48624669690616,
-0.9994492212522277, -0.032231638645109084, 0.007897823027072972, -2091.9865008569323,
0.007826347901904247, 0.0023431061004955257, 0.999966628509332, 10.446635558269918,
0.0,0.0,0.0,1.0]}
{"ex_paramter": [
[-0.03224906846459613, 0.999477679252545, -0.0020895595499665115, 475.48624669690616],
[-0.9994492212522277, -0.032231638645109084, 0.007897823027072972, -2091.9865008569323],
[0.007826347901904247, 0.0023431061004955257, 0.999966628509332, 10.446635558269918],
[0.0, 0.0, 0.0, 1.0]]}
旋转的算法--其中一种：欧拉角。Pitch、Yaw、Roll
沿着机身右方轴（Unity中的+X）进行旋转，称为pitch，中文叫俯仰。
沿着机头上方轴（Unity中的+Y）进行旋转，称为Yaw，中文叫偏航。
沿着机头前方轴（Unity中的+Z）进行旋转，称为Roll，中文叫桶滚。
欧拉角的“万向节死锁”问题，是由于欧拉旋转定义本身造成的。这种围绕选旋转前固定轴的先Z、再X、再Y的旋转操作，
与其最终所预期的三个轴向可以旋转的结果并非一定是一对一的映射。
某些情况下是多对一的映射，造成一些旋转自由度的缺失，也就是“死锁”。
Ros 中
变换矩阵 == 旋转矩阵 平移矩阵
四元数 -> 旋转矩阵
python_四元数q转旋转矩阵R
orientation = pose.orientation
position = pose.position
orientation.x, orientation.y, orientation.z, orientation.w
position.x, position.y, position.z
r_matrix = R.from_quat([orientation.x, orientation.y, orientation.z, orientation.w]).as_matrix()
t_array = np.asarray([position.x, position.y, position.z]).reshape(3, 1)
zero_line = np.asarray([0, 0, 0, 1])
rt_matrix = np.vstack((np.hstack((r_matrix, t_array)), zero_line))
pose_file_obj.write(json.dumps({"ex_paramter":np.asarray(rt_matrix).reshape(4,4).tolist()}))
pose_file_obj.write(json.dumps({"pose_matrix": rt_matrix.reshape(16).tolist()}))
示例
from scipy.spatial.transform import Rotation as R
Rq=[-0.35, 1.23e-06, 4.18e-08, 0.39]
Rm = R.from_quat(Rq)
rotation_matrix = Rm.as_matrix()
print('rotation:\n',rotation_matrix)
###
旋转矩阵->四元数
###四元数到欧拉角
def quart_to_rpy(x, y, z, w):
roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
pitch = math.asin(2 * (w * y - x * z))
yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (z * z + y * y))
return roll, pitch, yaw
# 使用 tf 库
import tf
(r, p, y) = tf.transformations.euler_from_quaternion([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w])
坐标系变换
#世界坐标转lidar坐标
for p in points:
xyz_array = np.asarray([p.x, p.y, p.z, 1]).reshape(4, 1)
rt_array = np.asarray(rt_matrix).reshape(4, 4)
new_xyz_array = np.dot(np.linalg.inv(rt_array), xyz_array)
new_xyz = new_xyz_array.reshape(4).tolist()
Python 点云处理
numpy类型的矩阵
# #二维数组
[][]和[,]
a_matrix =np.array([[1,2,3],[4,5,6]])
print(a_matrix[1][0:]) # [x][0:] 获取第x行的全部元素（单行）
array类型(N维数组ndarray)和矩阵类型matrix
matrix其实也是一种array，只不过是维数为2的特殊array
Python的numpy
##索引和切片
整数索引--name索引--布尔索引
将数据复制到新的数组
切片：切片方式：①通过内置的 slice(start, stop, step) 函数；②通过冒号分隔切片参数 start:stop:step
数组的切片是原数组的视图，任何对视图的修改都会反映到原数组上
索引切片
索引用来对单个成员(元素)进行访问，切片则是对一定范围内的成员(元素)进行访问
切片通过冒号在中括号内把相隔的两个索引查找出来[0: 10] 左含右不含【）
二维数组的索引和切片
通过切片返回一个数组-步长默认为1
数组名[维度1起始索引:终止索引:步长,维度2起始索引:终止索引:步长]
通过索引访问一个元素
数组名[第一个维度的索引值][第二个维度的索引值]
函数的解释
###1.点积
a.T代表矩阵的转置
a.dot(b)或np.dot(a,b)代表矩阵a与矩阵b做矩阵乘法
np.dot(a,a.T) 或者a.dot(a.T)
x为 m×n 阶矩阵，y为 n×p 阶矩阵，则相乘的结果 result 为 m×p 阶矩阵
np.linalg.inv(matrix) ###就是求逆矩阵
如一个点乘以一个矩阵后得到了一个新的点的位置，如果想通过这个点再获得矩阵转换前的位置，那我们就需要乘以这个矩阵的逆矩阵
###2.数组的组合与分隔
水平组合
hastck:将数组水平拼接到一起
用法：hastck((数组A，数组B))水平拼接数组A和B
垂直组合
vstack:将数组垂直拼接到一起
用法：vstack(数组A，数组B)垂直拼接数组A和B
矩阵
|-------> 变换矩阵列== 旋转矩阵（Rotation Matrix） + Translation 平移
| 1 0 0 x | \
| 0 1 0 y | }-> 左边是一个3阶的单位阵(无旋转) 3x3 matrix
| 0 0 1 z | /
| 0 0 0 1 | -> 这一行用不到 (这一行保持 0,0,0,1)
##尺度矩阵（Scale Matrix） 用于缩放 --scale是一个3D向量，表示每个轴的缩放尺度。
位姿信息构建转换矩阵
from scipy.spatial.transform import Rotation as R
import numpy as np
Rr = R.from_quat([orientation.x, orientation.y, orientation.z, orientation.w])
Rr_matrix = Rr.as_matrix()
Tt_array = np.asarray([position.x, position.y, position.z]).reshape(3, 1)
zero_line = np.asarray([0, 0, 0, 1])
rt_matrix = np.vstack((np.hstack((Rr_matrix, Tt_array)), zero_line)).reshape(4,4)
#提取
Rr =np.asarray(rt_matrix[0:3, 0:3])
#Tt = np.asarray([ rt_matrix[0][3], rt_matrix[1][3], rt_matrix[2][3]])
Tt = np.asarray(rt_matrix[:3, 3])
###坐标系变换
for p in points:
xyz_array = np.asarray([p.x, p.y, p.z, 1]).reshape(4, 1)
new_xyz_array = np.dot(np.linalg.inv(rt_array), xyz_array)
new_xyz = new_xyz_array.reshape(4).tolist()
[new_xyz[0], new_xyz[1], new_xyz[2], p.intensity, p.ring, p.timestamp]
###坐标系变换
for p in points:
point_ = np.dot(Rr.T, [p[0],p[1],p[2]]- Tt)
new_point_x = point_[0]
new_point_y = point_[1]
new_point_z = point_[2]
h = struct.pack('ffff', new_point_x, new_point_y, new_point_z, p[3])
### 旋转矩阵的逆等于其转置矩阵
### 正交矩阵的逆（inverse）等于正交矩阵的转置（transpose） np.linalg.det() 矩阵的行列式
#变换数据
###分块计算
new_xyz = np.dot(transformation_matrix,old_xyz.T)
new_array=np.concatenate((new_xyz.T[:,:3],old_array[:,3:]),axis=1)
数值计算库 Eigen
矩阵运算
使用Eigen进行坐标变换
转置 m.transpose()
1.点积（内积）
Eigen::Vector3d v(1,2,3);
Eigen::Vector3d w(0,1,2);
v.dot(w);
2.变换矩阵 初始化 一般先定义为单位变换矩阵，然后再赋值旋转和平移：
Eigen::Matrix3d m;
m<<0.999555,-0.00658702,0.0290829,0.0030476,0.992756,0.120107,-0.0296634,-0.119964,0.992335;
Eigen::Vector3d v;
v<<0.0506837,-0.0582575,0.140693;
//构造变换矩阵
Eigen::Isometry3d Tcw=Eigen::Isometry3d::Identity();;
Tcw.rotate(m);
Tcw.pretranslate(v);
示例
#include <Eigen/Dense>
using Eigen::Vector3d;
参考
点云数据常用处理：python实现 https://blog.csdn.net/taifyang/article/details/124365265
https://blog.csdn.net/shyjhyp11/article/details/111654692
Eigen is a C++ template library for linear algebra: matrices, vectors, numerical solvers, and related algorithms.
http://eigen.tuxfamily.org/index.php?title=Main_Page
C++矩阵库 Eigen 简介 https://blog.51cto.com/u_6725876/5134436