1. 视觉SFM理论
假设滑窗中一共有11帧，首先需要选取一个枢纽帧，利用枢纽帧和最后一帧通过对极约束求出这两帧之间的相对位姿。对枢纽帧的要求是：一方面要求枢纽帧离最后一帧尽可能远，因为离最后一帧比较近时，二者之间的平移比较小，使得三角化精度较差，甚至会无法进行对极约束（平移为零的情况）。另一方面，要求枢纽帧与最后一帧之间不能距离太远，否则会使得二者之间关联的特征点比较少，导致求解出的位姿可信度不高，并且无法三角化出尽可能多的点。因此，在保证匹配点足够多的情况下，枢纽帧离最后一帧应该尽可能远，如下图中选取第4帧作为枢纽帧。
将枢纽帧设为参考帧，其位姿设置：旋转为单位阵 I I I，平移为0。通过对极几何求出枢纽阵(4)和最后一帧(10)之间的相对位姿R和t（尺度未知）。由于第4帧的位姿为 ( I , 0 ) (I,0) (I,0)，那么第10帧的位姿就是二者之间的相对位姿 ( R , t ) (R,t) (R,t)。然后，在已知位姿之后，通过三角化求出这两帧之间的共视点（带尺度的）。由于光流追踪的性质，求出的第4帧与第10帧之间的共视点也会被中间的第5—9帧所看到，那么在已知这些共视点（3D）和第4帧的关键点（2D）之后，通过PnP的方法（3D-2D）求解出第5帧的位姿。按照同样的方法，通过三角化获取更多的共视点，通过PnP求出枢纽帧(4)和最后一帧(10)之间的每一帧(5—9)的位姿。
枢纽帧(4)与第一帧(0)之间的位姿和共视点求取按照同样的方法，先根据对极几何求出第0帧和第4帧的相对位姿，由于第4帧的位姿为 ( I , 0 ) (I,0) (I,0)，那么求出的相对位姿就是第0帧的位姿。然后通过三角化求出第0帧与第4帧之间的共视点，根据光流追踪的性质，这些点都会被第1—3帧所看到。然后通过PnP的方法求出第1帧的位姿，以此类推，通过三角化获取更多的共视点，通过PnP求出第2、3帧的位姿。
通过对滑窗进行上述操作，可求出滑窗中每一帧的位姿和所看到的3D共视点。
2. 获取枢纽帧并计算相对位姿relativePose
（1）大致流程
- 遍历滑窗中的每一帧，通过f_manager获取遍历到的当前帧与最后一帧之间的匹配对
- 通过f_manager直接获取这一匹配对的共视点个数，在共视点个数超过20个的前提下，执行如下操作：
- 获取每个共视点在各自帧中的像素坐标，根据像素坐标计算出其视差，进而计算出所有共视点的平均视差
- 结合虚拟相机焦距（460像素/mm），满足平均视差超过30个像素（这样保证枢纽帧与最后一帧具有足够的距离）的当前帧作为枢纽帧
- 然后，通过对极约束，调用solveRelativeRT计算出枢纽帧与滑窗中最后一帧之间的相对位姿
（2）代码实现
/**
* @brief 寻找滑窗内一个帧作为枢纽帧，要求和最后一帧既有足够的共视也要有足够的视差
* 并求出枢纽帧与滑窗中最后一帧之间的相对位姿
*
* @param[in] relative_R
* @param[in] relative_T
* @param[in] l
* @return true
* @return false
*/
bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)
{
// 优先从最前面开始遍历，确定哪一帧能够选定为枢纽帧
for (int i = 0; i < WINDOW_SIZE; i++)
{
vector<pair<Vector3d, Vector3d>> corres;
// corres表示第i帧与最后一帧WINDOW_SIZE同时观测到的3D点在各自归一化坐标系下的投影点
corres = f_manager.getCorresponding(i, WINDOW_SIZE);
// 要求共视的特征点足够多
if (corres.size() > 20) // 若第i帧与最后一帧的共视点个数 大于 20
{
double sum_parallax = 0;
double average_parallax;
for (int j = 0; j < int(corres.size()); j++)
{
// 第j个共视点在当前第i帧中的像素坐标(u, v)
Vector2d pts_0(corres[j].first(0), corres[j].first(1));
// 第j个共视点在最后一帧中的像素坐标(u, v)
Vector2d pts_1(corres[j].second(0), corres[j].second(1));
double parallax = (pts_0 - pts_1).norm(); // 计算二者之间的视差
sum_parallax = sum_parallax + parallax; // 求所有共视点的视差和
}
// 计算两帧之间共视点的平均视差
average_parallax = 1.0 * sum_parallax / int(corres.size());
/*
要求平均视差在虚拟相机的焦距(460像素/mm)中 大于 30，就认为当前帧i与最后一帧之间具有足够的视差
不同相机的视差判断阈值就不一样，这里使用虚拟相机焦距，使得视差阈值统一为30个像素
然后，利用对极约束，通过本质矩阵恢复枢纽帧和最后一帧之间的相对位姿 R t ———— solveRelativeRT
*/
if(average_parallax * 460 > 30 &&
m_estimator.solveRelativeRT(corres, relative_R, relative_T))
{
// 枢纽帧: 既与最后一帧有足够的视差，又能够使用对极约束成功计算出与最后一帧之间的相对位姿
l = i;
return true;
}
}
}
return false;
}
（3）对极约束求解相对位姿
大致流程
- 首先，在共视特征点匹配对个数不少于15的前提下，获取输入的两关键帧的共视特征点匹配对
- 根据两关键帧上的匹配像素点，调用opencv函数cv::findFundamentalMat计算本质矩阵E
- 调用opencv函数cv::recoverPose，根据本质矩阵恢复出位姿R和t，并返回内点个数（用于衡量匹配点中满足求解出的R和t的点的个数）
- 求解出的位姿是cv::Mat格式的R21和t21，先将其转换成eigen格式，然后通过下面推导出的公式，将其转换成R12和t12。并且，只有当内点个数超过12时，求解出的R12和t12才有效。
假设第一帧的位姿为单位矩阵，要求第二帧位姿，则相对位姿 R 21 R_{21} R21和 t 21 t_{21} t21就是第二帧的位姿。二者之间的转换关系推导如下：
P 1 = R 12 ⋅ P 2 + t 12 P 2 = R 21 ⋅ P 1 + t 21 ⇒ R 12 T ⋅ P 1 = P 2 + R 12 T ⋅ t 12 P 2 = R 12 T ⋅ P 1 − R 12 T ⋅ t 12 ⇒ R 21 = R 12 T t 21 = − R 12 T ⋅ t 12 ⇒ R 12 = R 21 T t 12 = − R 21 T ⋅ t 21 P_1 = R_{12} \cdot P_2 + t_{12} \ \ \ \ \ \ \ \ \ \ P_2 = R_{21} \cdot P_1 + t_{21} \\ \Rightarrow \\ R_{12}^T \cdot P_1 = P_2 + R_{12}^T \cdot t_{12} \ \ \ \ \ P_2 = R_{12}^T \cdot P_1 - R_{12}^T\cdot t_{12} \\ \Rightarrow \\ R_{21} = R_{12}^T\ \ \ \ \ t_{21} = -R_{12}^T \cdot t_{12} \\ \Rightarrow \\ R_{12} = R_{21}^T\ \ \ \ \ t_{12} = -R_{21}^T \cdot t_{21} P1=R12⋅P2+t12 P2=R21⋅P1+t21⇒R12T⋅P1=P2+R12T⋅t12 P2=R12T⋅P1−R12T⋅t12⇒R21=R12T t21=−R12T⋅t12⇒R12=R21T t12=−R21T⋅t21
代码实现
/**
* @brief 根据两帧匹配对求解R和带尺度的t
*
* @param[in] corres 两帧的匹配对
* @param[out] Rotation 求解出的相对旋转
* @param[out] Translation 求解出的相对平移
* @return true
* @return false
*/
bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)
{
if (corres.size() >= 15) // 两帧的匹配点对至少要有15个
{
vector<cv::Point2f> ll, rr; // 获取匹配点对的坐标
for (int i 
最低0.47元/天 解锁文章
1154
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
