两个 Eigen::Vector3d 如何求相对位姿

两个 Eigen::Vector3d 可以表示两个三维空间中的点的位置，但无法直接求出相对位姿。要求出相对位姿，需要知道这两个点在某个参考坐标系下的坐标。

假设这两个点分别为 A 和 B，在某个参考坐标系下的坐标分别为 $P_A$ 和 $P_B$。那么可以通过以下步骤求出它们之间的相对位姿：

1. 将点 A 和点 B 的坐标分别减去参考坐标系的原点坐标，得到它们相对于参考坐标系原点的坐标 $P_A'$ 和 $P_B'$。
2. 计算从点 A 到点 B 的向量 $v = P_B' - P_A'$。
3. 计算向量 $v$ 的旋转角度和旋转轴，可以使用 Eigen 库的 AngleAxisd 类来表示旋转。
4. 根据旋转角度和旋转轴，构造出相对位姿的变换矩阵，可以使用 Eigen 库的 Isometry3d 类来表示变换矩阵。

代码示例：

#include <Eigen/Core>
#include <Eigen/Geometry>

Eigen::Vector3d A, B; // 两个点的坐标
Eigen::Vector3d P_A = ...; // 点 A 在参考坐标系下的坐标
Eigen::Vector3d P_B = ...; // 点 B 在参考坐标系下的坐标

// 将点 A 和点 B 的坐标转换为相对于参考坐标系原点的坐标
Eigen::Vector3d P_A_prime = P_A - Eigen::Vector3d::Zero();
Eigen::Vector3d P_B_prime = P_B - Eigen::Vector3d::Zero();

// 计算从点 A 到点 B 的向量
Eigen::Vector3d v = P_B_prime - P_A_prime;

// 计算旋转角度和旋转轴
Eigen::AngleAxisd rotation(v.normalized(), v.norm());

// 构造出相对位姿的变换矩阵
Eigen::Isometry3d relative_pose = Eigen::Isometry3d::Identity();
relative_pose.rotate(rotation);
relative_pose.translation() = P_A;

这样就可以得到从点 A 到点 B 的相对位姿了。