一、坐标旋转

 坐标旋转包含绕x、y、z轴旋转，在右手坐标系中，x-翻滚(roll)，y-俯仰(pitch)，z-航向(yaw)。如果想详细了解，可以网络搜索

在PCL中，从baseLink到map的转换关系为:先绕x轴旋转,在绕y轴旋转,最后绕z轴旋转,即左乘为Rzyx, 然后在平移t. 从map到baseLink的转换关系正好相反,先平移-t, 然后绕z轴旋转,在绕y轴,最后绕x轴. 注意,此时旋转的角度与baseLink到map的正好相反.

当前车体的位置即姿态x, y, z, roll, pitch, yaw。通过pcl::getTransformation获取从车体坐标系到大地坐标系的旋转矩阵。 

Eigen::Matrix3f getRomateZYXMat(float yaw, float pitch, float roll){float cz = cos(yaw);float sz = sin(yaw);float cx = cos(roll);float sx = sin(roll);float cy = cos(pitch);float sy = sin(pitch);Eigen::Matrix3f Rx, Ry, Rz, mat;Rx << 1,   0, 0,0,  cx, -sx,0,  sx, cx;Ry << cy, 0, sy,0, 1, 0,-sy, 0, cy;Rz << cz, -sz, 0,sz,  cz, 0,0 ,   0, 1;mat = Rx * Ry * Rz;return mat;
}

 

在三维点云中，

 

这个旋转公式可以，有效的解释其选择公式代码

如下

cx = cos(x(1));cy = cos(x(2));cz = cos(x(3));sx = sin(x(1));sy = sin(x(2));sz = sin(x(3));R1 = [cy*cz, sx*sy*cz-cx*sz, cx*sy*cz+sx*sz;cy*sz, cx*cz+sx*sy*sz, cx*sy*sz-sx*cz;-sy,          sx*cy,          cx*cy];

代码理解完毕