Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

lego_loam中这个方法,为什么先绕z旋转,再绕x旋转,后绕y旋转? #13

Open
oocomeon opened this issue Aug 5, 2024 · 0 comments

Comments

@oocomeon
Copy link

oocomeon commented Aug 5, 2024

void pointAssociateToMap(PointType const * const pi, PointType * const po)
{
    // 进行6自由度的变换,先进行旋转,然后再平移
    // 主要进行坐标变换,将局部坐标转换到全局坐标中去	

    // 先绕z轴旋转
    //     |cosrz  -sinrz  0|
    //  Rz=|sinrz  cosrz   0|
    //     |0       0      1|
    // [x1,y1,z1]^T=Rz*[pi->x,pi->y,pi->z]
    float x1 = cYaw * pi->x - sYaw * pi->y;
    float y1 = sYaw * pi->x + cYaw * pi->y;
    float z1 = pi->z;

    // [x2,y2,z2]^T=Rx*[x1,y1,z1]
    //    |1     0        0|
    // Rx=|0   cosrx -sinrx|
    //    |0   sinrx  cosrx|
    float x2 = x1;
    float y2 = cRoll * y1 - sRoll * z1;
    float z2 = sRoll * y1 + cRoll * z1;

    // 最后再绕Y轴旋转,然后加上平移
    //    |cosry   0   sinry|
    // Ry=|0       1       0|
    //    |-sinry  0   cosry|
    po->x = cPitch * x2 + sPitch * z2 + tX;
    po->y = y2 + tY;
    po->z = -sPitch * x2 + cPitch * z2 + tZ;
    po->intensity = pi->intensity;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant