三维点云中的坐标变换（只讲关键部分）

 

一、坐标旋转

 坐标旋转包含绕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];

代码理解完毕