已知相机的旋转矩阵求相机的平移向量C++

假设已知相机的旋转矩阵为R，我们要求相机的平移向量t。

根据相机模型，可以得到以下方程：Xc = R*Xw + t

其中Xc为世界坐标系下的点在相机坐标系下的坐标，Xw为世界坐标系下的点的坐标。

将方程改写为矩阵形式：[Xc;1] = [R t]*[Xw;1]

其中[Xc;1]和[Xw;1]表示包含变量和常数的列向量，[R t]表示包含旋转矩阵和平移向量的矩阵。

我们可以通过以下代码求解平移向量t：

#include <Eigen/Core>
#include <Eigen/Dense>

Eigen::Matrix3d R; // 旋转矩阵
Eigen::Vector3d t; // 平移向量

// 假设已知R为旋转矩阵，需要求解t
Eigen::Matrix<double, 3, 4> Rt; // [R t]
Rt.block<3, 3>(0, 0) = R;
Rt.block<3, 1>(0, 3) = t;

Eigen::Vector3d Xc; // 相机坐标系下的点的坐标
Eigen::Vector3d Xw; // 世界坐标系下的点的坐标

// 求解平移向量t
t = (Rt.inverse() * Xc.homogeneous()).homogeneous().block<3, 1>(0, 0) - Xw;

其中，Xc.homogeneous()和Xw.homogeneous()表示将向量转换为齐次坐标形式，即在向量末尾添加一个1，以便进行矩阵乘法运算。t = (Rt.inverse() * Xc.homogeneous()).homogeneous().block<3, 1>(0, 0)表示先将Xc转换为齐次坐标形式，然后用Rt的逆矩阵乘以Xc，最后再将结果转换为非齐次坐标形式。最后再减去Xw，即可得到平移向量t。