使用 Eigen 在 C++ 中的不同参考中表达向量和方向

Question

假设我有三个推荐人（A、B 和 C）

我知道以下值：

B 在 A 中的位置（作为 Eigen::Vector3d）
B 在 A 中的方向（作为 Eigen::Quaterniond）
C 在 B 中的位置（作为 Eigen::Vector3d）
C 在 B 中的方向（作为 Eigen::Quaterniond）

如何使用 C++ 和 Eigen 在 C 中找到 A 的位置和方向？

Eigen::Vector3d p_B_in_A = Eigen::Vector3d(...);
Eigen::Quaterniond q_B_in_A = Eigen::Quaterniond(...);
Eigen::Vector3d p_C_in_B = Eigen::Vector3d(...);
Eigen::Quaterniond q_C_in_B = Eigen::Quaterniond(...);

Eigen::Vector3d p_A_in_C = ???
Eigen::Quaterniond q_A_in_C = ???

Answer 1

我以下面的代码为例回答我自己的问题：

Eigen::Affine3d B2A = Eigen::Affine3d::Identity();
B2A.translation() << 2 , -1 , 1;
B2A.linear() << std::sqrt(2)/2,  0, std::sqrt(2)/2,
               -std::sqrt(2)/2,  0, std::sqrt(2)/2,
                        0        , -1,         0;

Eigen::Affine3d C2B = Eigen::Affine3d::Identity();
C2B.translation() << 0, 1, 3*sqrt(2);
C2B.linear() <<  1, 0, 0,
                 0,-1, 0,
                 0, 0,-1;

Eigen::Affine3d A2C = C2B.inverse() * B2A.inverse();
// At this point, the orientation of A in C can be found in
// A2C.linear() as a 3x3 rotation matrix. the position of A 
// in C can be found in A2C.translation() as a Vector3d.

B2A表示B在A坐标系中的位置和方向。同理，C2B表示B在C中的位置和方向。通过对二次变换进行逆运算，可以求出A在C中的位置和方向。使用 Eigen 中的 Affine 对于进行 space 转换以及结合平移和旋转非常有用。

使用 Eigen 在 C++ 中的不同参考中表达向量和方向

Express Vector and orientation in a different referencial in C++ with Eigen

c++

eigen