slam中frame变换连乘顺序

记录一下，在坐标变换中踩到的一个坑

在slam中进行坐标变换时,若想求得变换$T_2^1$，有下面两种方式可以使用:

1. $$T_2^1=T_0^1\ast T_2^0$$

2. $$T_2^1=T_2^0\ast T_0^1$$

然而当我们使用如下代码进行验证时

#include <Eigen/Geometry>
#include <iostream>

int main()
{
    Eigen::Quaternionf base_1_to_0(Eigen::AngleAxisf(M_PI / 3, Eigen::Vector3f::UnitX()) * Eigen::AngleAxisf(M_PI / 3, Eigen::Vector3f::UnitY()) * Eigen::AngleAxisf(M_PI / 3, Eigen::Vector3f::UnitZ()));
    Eigen::Quaternionf base_2_to_0(Eigen::AngleAxisf(M_PI / 6, Eigen::Vector3f::UnitX()) * Eigen::AngleAxisf(M_PI / 6, Eigen::Vector3f::UnitY()) * Eigen::AngleAxisf(M_PI / 6, Eigen::Vector3f::UnitZ()));

    auto T0 = (base_1_to_0.inverse() * base_2_to_0).normalized();
    auto T1 = (base_2_to_0 * base_1_to_0.inverse()).normalized();

    std::cout<<"T0:"<<T0.w()<<" "<<T0.x()<<" "<<T0.y()<<" "<<T0.z() <<std::endl;
    std::cout<<"T1:"<<T1.w()<<" "<<T1.x()<<" "<<T1.y()<<" "<<T1.z() <<std::endl;

    Eigen::Vector3f vec(1,1,1);

    std::cout << "T0 matrix:\n" << T0.toRotationMatrix() << std::endl;
    std::cout << "T1 matrix:\n" << T1.toRotationMatrix() << std::endl;

    std::cout << "T0 vector:\n"
              << T0.toRotationMatrix() * vec << std::endl;
    std::cout << "T1 vector:\n"
              << T1.toRotationMatrix() * vec << std::endl;

    std::cout << "T0 eular:\n"
              << T0.toRotationMatrix().eulerAngles(0,1,2)/M_PI*180 << std::endl;
    std::cout << "T1 eular:\n"
              << T1.toRotationMatrix().eulerAngles(0,1,2)/M_PI*180 << std::endl;

    return 0;
}

却得到如下结果:

T0:0.853854 -0.306186 -0.0473672 -0.418258
T1:0.853854 -0.418258 -0.0473672 -0.306186
T0 matrix:
 0.645633  0.743269   0.17524
-0.685256   0.46262    0.5625
 0.337019 -0.483253  0.808013
T1 matrix:
 0.808013    0.5625   0.17524
-0.483253   0.46262  0.743269
 0.337019 -0.685256  0.645633
T0 vector:
 1.56414
0.339864
0.661779
T1 vector:
 1.54575
0.722636
0.297395
T0 eular:
145.156
169.907
130.979
T1 eular:
130.979
169.907
145.156

可以看出两种方式所得到的$T_2^1$结果是不一样的。

那么我们应该使用那种方式进行计算呢？

我们知道$T_2^1$的变换是:将$Fram{e}_2$下的特征变换到$Fram{e}_1$下进行表示，设$Fram{e}_2$的特征为点$P^2$,我们分别用上面的两种方式去对$P^2$做变换:

1. $$P^1=T_2^1\ast P^2=T_0^1\ast T_2^0\ast P^2$$
2. $$P^1=T_2^1\ast P^2=T_2^0\ast T_0^1\ast P^2$$

可以看到在公式2的尾部 $T_0^1\ast P^2$ 是没有实际的变换意义的，所以变换要使用第一种方法，即 所获得的最终变换的上标作为变换连乘的起始上标，最终变换的下标作为连乘变换的终止下标，中间消元部分的方向是由左下到右上进行消元。

结论:使用方法1: $T_2^1=T_0^1\ast T_2^0$。