legoloam中transformAssociateToMap函数讨论

最新推荐文章于 2024-05-09 15:52:23 发布

豆皮卷香菜

最新推荐文章于 2024-05-09 15:52:23 发布

阅读量1.1k

点赞数 5

分类专栏： slam 文章标签：算法 slam

本文链接：https://blog.csdn.net/l898985121/article/details/114844864

版权

slam 专栏收录该内容

20 篇文章 8 订阅

订阅专栏

文章目录

1. 说明
2. 主要测试代码
3. 完整测试代码

1. 说明

1.1 参考资料

主要参考了知乎文章，并做了一些符合调整，符合作者本人理解
https://zhuanlan.zhihu.com/p/159525107

1.2 transformAssociateToMap函数说明

1.2.1 函数作用

通过laserOdometry可以获得本帧与上一帧之间的变换关系，利用这个变换关系，给出一个待优化的初始值。如下图所示
有世界坐标系x0，通过laserOdom可以得到上一时刻车辆位置x1，本时刻车辆位置x2，上一时刻位置x1经过优化后可以得到x3，本函数的目的就是通过x1和x2之间的变换t1，以及x3，预测x4的初始位置。

在这里插入图片描述

1.2.2 计算方法

做法是：

直接通过x1和x2计算出t1，
然后将t1叠加到x3上，得到x4的初始位置

通过laserOdom可以计算出上一时刻车辆位姿在世界坐标系下的表示，记作 $^0_1T$ ，
通过laserOdom可以计算出本时刻车辆位姿在世界坐标系下的表示，记作 $^0_2T$ ，
其中 $^0_1T$ 和 $^0_2T$ 均为4x4齐次变换阵（homogeneous transformation）
则可直接得到 $t1=^1_0T \cdot ^0_2T$ ，
令 $t 2 = t 1$ ，并且已知通过mapping可以获得x3在世界坐标系下的表示 $^0_3T$ 。
有：
$^0_4T =^0_3T \cdot ^1_0T \cdot ^0_2T$

2. 主要测试代码

完整的测试代码放在了最后。
下面一段是核心的实现部分。利用了eigen库进行实现，
t1矩阵就是上式中的 $^0_2T$ ，t2矩阵就是上式中的 $^0_1T$ ，t2.inverse是 $^1_0T$ ，t3是上式中的 $^0_3T$ ，然后再按照 t3 * t2.inverse() * t1将三个式子乘在一起，就可以得到我们想要的变换

    Eigen::Matrix4f t1 = roty(transformSum[0]) * rotx(transformSum[1]) * rotz(transformSum[2]);
    t1(0,3) = transformSum[3];t1(1,3) = transformSum[4];t1(2,3) = transformSum[5];

    Eigen::Matrix4f t2 = roty(transformBefMapped[0]) * rotx(transformBefMapped[1]) * rotz(transformBefMapped[2]);
    t2(0,3) = transformBefMapped[3];t2(1,3) = transformBefMapped[4];t2(2,3) = transformBefMapped[5];

    Eigen::Matrix4f t3 = roty(transformAftMapped[0]) * rotx(transformAftMapped[1]) * rotz(transformAftMapped[2]);
    t3(0,3) = transformAftMapped[3];t3(1,3) = transformAftMapped[4];t3(2,3) = transformAftMapped[5];

    Eigen::Matrix4f c = t3 * t2.inverse() * t1;
    std::cout << c <<std::endl;

通过与原先代码结果进行对比，可以验证这里推导的正确性

3. 完整测试代码

#include <Eigen/Dense>
#include <cmath>
#include <iostream>
//#include "utility"

float transformLast[6];
float transformSum[6];
float transformIncre[6];
float transformTobeMapped[6];
float transformBefMapped[6];
float transformAftMapped[6];


Eigen::Matrix4f rotx(float angle)
{
    Eigen::Matrix4f b1;
    b1 << 1, 0, 0, 0,
          0, cos(angle), -sin(angle), 0,
          0, sin(angle), cos(angle), 0,
          0, 0, 0, 1;
    return b1;
}
Eigen::Matrix4f roty(float angle)
{
    Eigen::Matrix4f b1;
    b1 << cos(angle), 0, sin(angle), 0,
          0, 1, 0, 0,
          -sin(angle), 0, cos(angle), 0,
          0, 0, 0, 1;
    return b1;
}
Eigen::Matrix4f rotz(float angle)
{
    Eigen::Matrix4f b1;
    b1 << cos(angle), -sin(angle), 0, 0,
          sin(angle), cos(angle), 0, 0,
          0, 0, 1, 0,
          0,0,0,1;
    return b1;
}
Eigen::Matrix4f translation();
void transformAssociateToMap()
{
    float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
    float y1 = transformBefMapped[4] - transformSum[4];
    float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);

    float x2 = x1;
    float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1;
    float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1;

    transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2;
    transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2;
    transformIncre[5] = z2;

    float sbcx = sin(transformSum[0]);
    float cbcx = cos(transformSum[0]);
    float sbcy = sin(transformSum[1]);
    float cbcy = cos(transformSum[1]);
    float sbcz = sin(transformSum[2]);
    float cbcz = cos(transformSum[2]);

    float sblx = sin(transformBefMapped[0]);
    float cblx = cos(transformBefMapped[0]);
    float sbly = sin(transformBefMapped[1]);
    float cbly = cos(transformBefMapped[1]);
    float sblz = sin(transformBefMapped[2]);
    float cblz = cos(transformBefMapped[2]);

    float salx = sin(transformAftMapped[0]);
    float calx = cos(transformAftMapped[0]);
    float saly = sin(transformAftMapped[1]);
    float caly = cos(transformAftMapped[1]);
    float salz = sin(transformAftMapped[2]);
    float calz = cos(transformAftMapped[2]);

    float srx = -sbcx * (salx * sblx + calx * cblx * salz * sblz + calx * calz * cblx * cblz) - cbcx * sbcy * (calx * calz * (cbly * sblz - cblz * sblx * sbly) - calx * salz * (cbly * cblz + sblx * sbly * sblz) + cblx * salx * sbly) - cbcx * cbcy * (calx * salz * (cblz * sbly - cbly * sblx * sblz) - calx * calz * (sbly * sblz + cbly * cblz * sblx) + cblx * cbly * salx);
    transformTobeMapped[0] = -asin(srx);

    float srycrx = sbcx * (cblx * cblz * (caly * salz - calz * salx * saly) - cblx * sblz * (caly * calz + salx * saly * salz) + calx * saly * sblx) - cbcx * cbcy * ((caly * calz + salx * saly * salz) * (cblz * sbly - cbly * sblx * sblz) + (caly * salz - calz * salx * saly) * (sbly * sblz + cbly * cblz * sblx) - calx * cblx * cbly * saly) + cbcx * sbcy * ((caly * calz + salx * saly * salz) * (cbly * cblz + sblx * sbly * sblz) + (caly * salz - calz * salx * saly) * (cbly * sblz - cblz * sblx * sbly) + calx * cblx * saly * sbly);
    float crycrx = sbcx * (cblx * sblz * (calz * saly - caly * salx * salz) - cblx * cblz * (saly * salz + caly * calz * salx) + calx * caly * sblx) + cbcx * cbcy * ((saly * salz + caly * calz * salx) * (sbly * sblz + cbly * cblz * sblx) + (calz * saly - caly * salx * salz) * (cblz * sbly - cbly * sblx * sblz) + calx * caly * cblx * cbly) - cbcx * sbcy * ((saly * salz + caly * calz * salx) * (cbly * sblz - cblz * sblx * sbly) + (calz * saly - caly * salx * salz) * (cbly * cblz + sblx * sbly * sblz) - calx * caly * cblx * sbly);
    transformTobeMapped[1] = atan2(srycrx / cos(transformTobeMapped[0]),
                                   crycrx / cos(transformTobeMapped[0]));

    float srzcrx = (cbcz * sbcy - cbcy * sbcx * sbcz) * (calx * salz * (cblz * sbly - cbly * sblx * sblz) - calx * calz * (sbly * sblz + cbly * cblz * sblx) + cblx * cbly * salx) - (cbcy * cbcz + sbcx * sbcy * sbcz) * (calx * calz * (cbly * sblz - cblz * sblx * sbly) - calx * salz * (cbly * cblz + sblx * sbly * sblz) + cblx * salx * sbly) + cbcx * sbcz * (salx * sblx + calx * cblx * salz * sblz + calx * calz * cblx * cblz);
    float crzcrx = (cbcy * sbcz - cbcz * sbcx * sbcy) * (calx * calz * (cbly * sblz - cblz * sblx * sbly) - calx * salz * (cbly * cblz + sblx * sbly * sblz) + cblx * salx * sbly) - (sbcy * sbcz + cbcy * cbcz * sbcx) * (calx * salz * (cblz * sbly - cbly * sblx * sblz) - calx * calz * (sbly * sblz + cbly * cblz * sblx) + cblx * cbly * salx) + cbcx * cbcz * (salx * sblx + calx * cblx * salz * sblz + calx * calz * cblx * cblz);
    transformTobeMapped[2] = atan2(srzcrx / cos(transformTobeMapped[0]),
                                   crzcrx / cos(transformTobeMapped[0]));

    x1 = cos(transformTobeMapped[2]) * transformIncre[3] - sin(transformTobeMapped[2]) * transformIncre[4];
    y1 = sin(transformTobeMapped[2]) * transformIncre[3] + cos(transformTobeMapped[2]) * transformIncre[4];
    z1 = transformIncre[5];

    x2 = x1;
    y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;
    z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;

    transformTobeMapped[3] = transformAftMapped[3] - (cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2);
    transformTobeMapped[4] = transformAftMapped[4] - y2;
    transformTobeMapped[5] = transformAftMapped[5] - (-sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2);
}


int main(int argc, char** argv)
{
    transformSum[0] = 0.0;
    transformSum[1] = 0.0;
    transformSum[2] = M_PI / 6.0;
    transformSum[3] = 10.0;
    transformSum[4] = 20.0;
    transformSum[5] = 0.0;

    transformBefMapped[0] = 0.0;
    transformBefMapped[1] = 0.0;
    transformBefMapped[2] = M_PI / 4.0;
    transformBefMapped[3] = 8.0;
    transformBefMapped[4] = 17.0;
    transformBefMapped[5] = 0.0;

    transformAftMapped[0] = 0;
    transformAftMapped[1] = 0;
    transformAftMapped[2] = M_PI / 2.0;
    transformAftMapped[3] = 0;
    transformAftMapped[4] = 15.0;
    transformAftMapped[5] = 0;

    transformAssociateToMap();
    std::cout << transformTobeMapped[0] << " , "
              << transformTobeMapped[1] << " , "
              << transformTobeMapped[2] << " , "
              << transformTobeMapped[3] << " , "
              << transformTobeMapped[4] << " , "
              << transformTobeMapped[5] << std::endl;
    std::cout << "angle : "<< M_PI/2.0 - (M_PI/4 - M_PI/6)<<std::endl;

    Eigen::Matrix4f a = roty(transformTobeMapped[0]) * rotx(transformTobeMapped[1]) * rotz(transformTobeMapped[2]);
    a(0,3) = transformTobeMapped[3];a(1,3) = transformTobeMapped[4];a(2,3) = transformTobeMapped[5];
    std::cout << a << std::endl;
    std::cout << "------------------"<<std::endl;

    Eigen::Matrix4f t1 = roty(transformSum[0]) * rotx(transformSum[1]) * rotz(transformSum[2]);
    t1(0,3) = transformSum[3];t1(1,3) = transformSum[4];t1(2,3) = transformSum[5];

    Eigen::Matrix4f t2 = roty(transformBefMapped[0]) * rotx(transformBefMapped[1]) * rotz(transformBefMapped[2]);
    t2(0,3) = transformBefMapped[3];t2(1,3) = transformBefMapped[4];t2(2,3) = transformBefMapped[5];

    Eigen::Matrix4f t3 = roty(transformAftMapped[0]) * rotx(transformAftMapped[1]) * rotz(transformAftMapped[2]);
    t3(0,3) = transformAftMapped[3];t3(1,3) = transformAftMapped[4];t3(2,3) = transformAftMapped[5];

    Eigen::Matrix4f c = t3 * t2.inverse() * t1;
    std::cout << c <<std::endl;
    
    return 0;
}