图中流程图部分与代码对应密切，这里不做具体阐述，对应processImg()中的代码，这里不做罗列。重点介绍视觉惯导联合初始化部分的代码。
这部分是视觉惯导联合初始化的核心部分，主要涉及单目纯视觉的重建（SFM问题），以及视觉与IMU的对齐操作。本节主要分析SFM问题。
一、SFM中的特征点管理
首先是通过计算线加速度的标准差，判断 IMU 是否有充分运动激励，以进行初始化，然后是进行SFM所需要的特征点存入结构体SFMFeature tmp_feature;进行维护。这里涉及SFM中特征点管理方式，如下图所示：
如图所示，滑窗中的路标点被多个图像帧观测到，比图路标点1被图像帧0，1，2观测到，则将每一帧的id以及在该帧中路标点的属性以vector的形式存储在 observation中。
struct SFMFeature
{
bool state; //状态（是否被三角化）
int id; //路标点id
vector<pair<int, Vector2d>> observation; //所有观测到该特征点的图像帧ID和图像坐标
double position[3]; // 3d坐标
double depth; //深度
};二、选取参考帧并恢复相机运动
如图所示，滑窗中一共有0-10共10帧图像帧，而选取参考帧是比较关键的，参考帧是之后SFM的基础，参考帧需要和当前帧保持足够的视差，又要含有足够的内点，即共视点够多距离有够远，两者权衡之下，选择参考帧 l，选取参考帧的工作主要是由 relativePose() 函数完成的。 第l 帧是从滑动窗口中第1帧开始遍历，第一个满足与当前帧的平均视差足够大的帧；如下图所示，假设我们选取的参考帧是第5帧；
函数relativePose() 的作用，一方面是找到第 l 帧（参考帧），另一方面则是使用对极约束恢复参考帧到当前帧的对极约束，从而当前帧相对参考帧的位姿 $R,T$，为之后的纯视觉的三维重建奠定基础；
- 根据视差确定参考帧
/**
* @brief 保证有足够的视差，由E矩阵回复R t
*
* @param relative_R[out] 旋转矩阵
* @param relative_T[out] 变换矩阵
* @param l 索引：第 l 帧
* @return true
* @return false
*/
bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)
{
// find previous frame which contians enough correspondance and parallex with newest frame
// 优先从最前面开始
for (int i = 0; i < WINDOW_SIZE; i++)
{
//寻找第i帧(枢纽帧/参考帧)到窗口最后一帧的对应特征点
vector<pair<Vector3d, Vector3d>> corres;
corres = f_manager.getCorresponding(i, WINDOW_SIZE);
// 要求共视的特征点足够多
if (corres.size() > 20)
{
//计算平均视差
double sum_parallax = 0;
double average_parallax;
for (int j = 0; j < int(corres.size()); j++)
{
//第j个对应点在第i帧和最后一帧的(x,y)
Vector2d pts_0(corres[j].first(0), corres[j].first(1));
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());
//判断是否满足初始化条件：视差>30和内点数满足要求(大于12)
//solveRelativeRT()通过基础矩阵计算当前帧与第l帧之间的R和T,并判断内点数目是否足够
//同时返回窗口最后一帧（当前帧）到第l帧（参考帧）的relative_R，relative_T
if (average_parallax * 460 > 30 && m_estimator.solveRelativeRT(corres, relative_R, relative_T))
{
l = i;
ROS_DEBUG("average_parallax %f choose l %d and newest frame to triangulate the whole structure", average_parallax * 460, l);
return true;
}
}
}
return false;
}代码解析
程序设置一个 for循环，对滑窗中的所有帧进行遍历，使用getCorresponding() 函数求解每一帧和当前帧的共视点对，当共视点对大于 20 方可满足条件，满足共视点数足够多的条件之后，开始判断是视差，这里计算的是平均视差，因为两帧之间的共视点对不止一对，所以，遍历所有的共视点对，对每个共视点对求得视差取平均当平均视差大于 30 则满足称为参考帧的条件，此时我们就得到了参考帧 l;
- 使用对极约束，根据基本矩阵求解相机位姿
/**
* @brief 根据两帧匹配对求解R和带尺度的t
*
* @param corres[in] 匹配点对
* @param Rotation[out]
* @param Translation[out]
* @return true
* @return false
*/
bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)
{
if (corres.size() >= 15)
{
vector<cv::Point2f> ll, rr;
for (int i = 0; i < int(corres.size()); i++)
{
ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
}
cv::Mat mask;
/**
* Mat cv::findFundamentalMat( 返回通过RANSAC算法求解两幅图像之间的本质矩阵E
* nputArray points1, 第一幅图像点的数组
* InputArray points2, 第二幅图像点的数组
* int method = FM_RANSAC, RANSAC 算法
* double param1 = 3., 点到对极线的最大距离，超过这个值的点将被舍弃
* double param2 = 0.99, 矩阵正确的可信度
* OutputArray mask = noArray() 输出在计算过程中没有被舍弃的点
* )
*/
cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);
// 已经是归一化相机坐标系了，因此内参阵用单位阵
cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
cv::Mat rot, trans;
/**
* int cv::recoverPose ( 通过本质矩阵得到Rt，返回通过手性校验的内点个数
* InputArray E, 本质矩阵
* InputArray points1, 第一幅图像点的数组
* InputArray points2, 第二幅图像点的数组
* InputArray cameraMatrix, 相机内参
* OutputArray R, 第一帧坐标系到第二帧坐标系的旋转矩阵
* OutputArray t, 第一帧坐标系到第二帧坐标系的平移向量
* InputOutputArray mask = noArray() 在findFundamentalMat()中没有被舍弃的点
* )
*/
int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);
//cout << "inlier_cnt " << inlier_cnt << endl;
Eigen::Matrix3d R;
Eigen::Vector3d T;
// 把CV:Mat格式转换为Eigen
for (int i = 0; i < 3; i++)
{
T(i) = trans.at<double>(i, 0);
for (int j = 0; j < 3; j++)
R(i, j) = rot.at<double>(i, j);
}
// Opencv得到的是T21,这里换成T12
Rotation = R.transpose();
Translation = -R.transpose() * T;
// 内点数满足要求(大于12)
if(inlier_cnt > 12)
return true;
else
return false;
}
return false;
}代码解析
得到参考帧 l 之后，还需要利用函数 solveRelativeRT() 求取参考帧到当前帧的位姿 $R,T$ ，并且使用函数cv::recoverPose()，他它的作用是通过本质矩阵得到 $R,T$ ，返回通过手性校验的内点个数，并且内点个数也同时是 l 称为参考帧的必要条件，只有内点个数大于 12 才能有资格成为参考帧。
已知在参考帧的点为 $P^1$，当前帧点为$P^2$，则
$$ \begin{align*} P^1&=R_{12}\cdot P^2+T_{12}\\ P^2&=R_{21}\cdot P^1+T_{21} \end{align*}\tag{1} $$
其中，$R_{12},T_{12}$ 表示当前帧到参考帧的位姿，$R_{21},T_{21}$ 表示参考帧到当前帧的位姿，对公式$(1)$中的第二个式子两边同乘 $R_{21}^T$
$$ \begin{align*} R_{21}^TP^2&=P^1+R_{21}^TT_{21}\\ \Rightarrow P^1&=R_{21}^TP^2-R_{21}^TT_{21} \end{align*}\tag{2} $$
对应相等：
$$ \begin{align*} P^1&=R_{12}\cdot P^2+T_{12}\\ P^1&=R_{21}^TP^2-R_{21}^TT_{21} \end{align*}\tag{3} $$
因此可得：
$$ \begin{align*} R_{12}&=R_{21}^T\\ T_{12}&=-R_{21}^TT_{21} \end{align*}\tag{4} $$
由于，上面程序求得的是，当前帧相对参考帧的位姿$R_{21},T_{21}$，因此，参考帧相对当前帧的相对位姿就可以使用上述公式求取。
至此，我们通过对极约束求得了当前帧相对参考帧的位姿 $R,T$；但是，我们知道，对于单目相机来说，存在尺度不确定性，因此这个 $T$ 任意放缩，对极约束都是成立的，所以单目对极几何仅仅起到初始化的作用。
这一步为后面的 PnP过程奠定了基础，因为单目视觉在使用PnP之前，必须使用对极约束，PnP的过程中使用了初始化得到的三角点，这样PnP求出来的 $R,T$ 是唯一的，其尺度是和初始化的时候的 $T$ 是一样的。
三、SFM重建
铺垫了这么多，终于可以进行三维重建了：
/**
* @description: 纯视觉 sfm，求解窗口中所有图像帧的位姿 QT（相对于第 l 帧）和特征点坐标 sfm_tracked_points
* @param {int} frame_num frame_num=frame_count + 1=11，frame_num-1 表示滑窗内的关键帧的总数
* @param {Quaterniond*} q 恢复出来的滑窗中的各个姿态
* @param {Vector3d*} T 恢复出来的滑窗中的各个平移
* @param {int} l 参考帧的ids
* @param {Matrix3d} relative_R 当前帧到第 l 帧的 relative_R（旋转）
* @param {Vector3d} relative_T 当前帧到第 l 帧的 relative_T（平移）
* @param {vector<SFMFeature>} &sfm_f 用来做sfm特征点的集合
* @param {map<int, Vector3d>} &sfm_tracked_points 恢复出来的地图点
* @return {*}
*/
bool GlobalSFM::construct(int frame_num, Quaterniond* q, Vector3d* T, int l,
const Matrix3d relative_R, const Vector3d relative_T,
vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points)
{}在详细讲解视觉重建的过程之前，需要对VINS自定义的三角化函数以及求解PnP的函数做一定的了解，因为两者是三维重建的基础，这部分函数的位置：vins_estimator\src\initial\initial_sfm.cpp
3.1 三角化函数
/**
* @brief 根据两帧索引以及位姿计算对应特征点的三角化位置
*
* @param frame0 参考帧索引
* @param Pose0 参考帧位姿
* @param frame1 当前帧索引
* @param Pose1 当前帧位姿
* @param sfm_f 三角化的结果
*/
void GlobalSFM::triangulateTwoFrames(int frame0, Eigen::Matrix<double, 3, 4> &Pose0,
int frame1, Eigen::Matrix<double, 3, 4> &Pose1,
vector<SFMFeature> &sfm_f)
{
assert(frame0 != frame1);// 保证两帧图像是不同的
for (int j = 0; j < feature_num; j++)
{
if (sfm_f[j].state == true) //已经三角化了
continue;
bool has_0 = false, has_1 = false;
Vector2d point0;
Vector2d point1;
// 遍历该特征点的观测, 判断是否能被当前帧和参考帧同时看到
for (int k = 0; k < (int)sfm_f[j].observation.size(); k++)
{
// 被参考帧看到 has_0 = true
if (sfm_f[j].observation[k].first == frame0)
{
point0 = sfm_f[j].observation[k].second; // 去除特征点在该帧的观测
has_0 = true;
}
// 被当前帧看到 has_1 = true
if (sfm_f[j].observation[k].first == frame1)
{
point1 = sfm_f[j].observation[k].second;
has_1 = true;
}
}
// 如果被两帧同时看到
if (has_0 && has_1)
{
Vector3d point_3d;
// 将这个特征点进行三角化
triangulatePoint(Pose0, Pose1, point0, point1, point_3d);
sfm_f[j].state = true; // 三角化之后，三角化标志位置为true
sfm_f[j].position[0] = point_3d(0);
sfm_f[j].position[1] = point_3d(1);
sfm_f[j].position[2] = point_3d(2);
//cout << "trangulated : " << frame1 << " 3d point : " << j << " " << point_3d.transpose() << endl;
}
}
}/**
* @brief VINS三角化
*
* @param Pose0 第一帧的位姿
* @param Pose1 第二帧的位姿
* @param point0 第一帧的相机归一化坐标
* @param point1 第二帧的相机归一化坐标
* @param point_3d 路标点的世界坐标
*/
void GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
Vector2d &point0, Vector2d &point1, Vector3d &point_3d)
{
// 通过奇异值分解求解一个Ax = 0得到
Matrix4d design_matrix = Matrix4d::Zero();
design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0);
design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1);
design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0);
design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1);
Vector4d triangulated_point;
triangulated_point =
design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
// 齐次向量归一化
point_3d(0) = triangulated_point(0) / triangulated_point(3);
point_3d(1) = triangulated_point(1) / triangulated_point(3);
point_3d(2) = triangulated_point(2) / triangulated_point(3);
}3.2 求解PnP
/**
* @brief 对相邻帧求解PnP
*
* @param R_initial 上一帧的位姿作为i帧的初值，并在函数最后做更新，赋初值的意义在于，能够加速求解
* @param P_initial
* @param i 当前索引
* @param sfm_f 用来做sfm特征点的集合
* @return true
* @return false
*/
bool GlobalSFM::solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i,
vector<SFMFeature> &sfm_f)
{
vector<cv::Point2f> pts_2_vector; //世界坐标系下的控制点的坐标
vector<cv::Point3f> pts_3_vector; //图像坐标系下对应的控制点的坐标
for (int j = 0; j < feature_num; j++) // 遍历j个路标点
{
if (sfm_f[j].state != true) // false就是没有三角化的点，PnP是3d-2d，因此需要3d点
continue;
Vector2d point2d;
for (int k = 0; k < (int)sfm_f[j].observation.size(); k++) // 遍历路标点j 被观测到的所有observation
{
// vector<pair<int, Vector2d> > observation 保存的是Id和图像坐标
if (sfm_f[j].observation[k].first == i) // 判断与当前帧i有共同观测的帧
{
//记录图像坐标
Vector2d img_pts = sfm_f[j].observation[k].second;
cv::Point2f pts_2(img_pts(0), img_pts(1));
pts_2_vector.push_back(pts_2);
// 记录3d坐标
cv::Point3f pts_3(sfm_f[j].position[0], sfm_f[j].position[1], sfm_f[j].position[2]);
pts_3_vector.push_back(pts_3);
break;
}
}
}
if (int(pts_2_vector.size()) < 15)
{
printf("unstable features tracking, please slowly move you device!\n");
if (int(pts_2_vector.size()) < 10)
return false;
}
// eigen转cv
cv::Mat r, rvec, t, D, tmp_r;
cv::eigen2cv(R_initial, tmp_r);
cv::Rodrigues(tmp_r, rvec);
cv::eigen2cv(P_initial, t);
cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
bool pnp_succ;
/**
* @brief OpenCV求解PnP
*
* @param objectPoints pts_3_vector 世界坐标系下的控制点的坐标
* @param imagePoints pts_2_vector 在图像坐标系下对应的控制点的坐标
* @param cameraMatrix K 相机的内参矩阵
* @param distCoeffs D 相机的畸变系数
* @param rvec rvec 输出的旋转向量。使坐标点从世界坐标系旋转到相机坐标系
* @param tvec t 输出的平移向量。使坐标点从世界坐标系平移到相机坐标系
* @param flags 默认使用CV_ITERATIV迭代法
*/
pnp_succ = cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1);
if(!pnp_succ)
{
return false;
}
// cv转eigen
cv::Rodrigues(rvec, r);
//cout << "r " << endl << r << endl;
MatrixXd R_pnp;
cv::cv2eigen(r, R_pnp);
MatrixXd T_pnp;
cv::cv2eigen(t, T_pnp);
R_initial = R_pnp;
P_initial = T_pnp;
return true;
}3.3 SFM重建过程
第一步：先三角化第 l 帧（参考帧）与第 frame_num-1 帧（当前帧）的路标点
第二步：pnp 求解参考帧 $l$ 到第 $l+1$ 开始的每一帧的变换矩阵 R_initial, P_initial，保存在 Pose 中
for (int i = l; i < frame_num - 1 ; i++)
{
// solve pnp
if (i > l)
{
// 这是依次求解，因此上一帧的位姿是已知量
Matrix3d R_initial = c_Rotation[i - 1];
Vector3d P_initial = c_Translation[i - 1];
if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
return false;
c_Rotation[i] = R_initial;
c_Translation[i] = P_initial;
c_Quat[i] = c_Rotation[i];
Pose[i].block<3, 3>(0, 0) = c_Rotation[i]; //相对当前帧[frame_num-1]的旋转
Pose[i].block<3, 1>(0, 3) = c_Translation[i];//相对当前帧[frame_num-1]的平移
}
//G_ Step1: 先三角化第 l 帧（参考帧）与第 frame_num-1 帧（当前帧）的路标点
// triangulate point based on the solve pnp result
// 根据PnP的结果，参考与当前帧进行三角化处理
triangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);
}第三步：对第 l 帧与 l+1 l+2 ... frame_num -2 的每一帧再进行三角化
考虑到前两步执行之后，会有些特征点不被最后一帧（当前帧）看到，因此，这一步将参考帧与之后的帧再次进行三角化。418133364418133364418133364
for (int i = l + 1; i < frame_num - 1; i++)
triangulateTwoFrames(l, Pose[l], i, Pose[i], sfm_f);第四步：PNP 求解参考帧到从第 l-1 到第 0 帧的每一帧之间的变换矩阵，并进行三角化
for (int i = l - 1; i >= 0; i--)
{
//solve pnp
// 这种情况就是最后一帧先求解出来
Matrix3d R_initial = c_Rotation[i + 1];
Vector3d P_initial = c_Translation[i + 1];
if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
return false;
c_Rotation[i] = R_initial;
c_Translation[i] = P_initial;
c_Quat[i] = c_Rotation[i];
Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
Pose[i].block<3, 1>(0, 3) = c_Translation[i];
//triangulate
triangulateTwoFrames(i, Pose[i], l, Pose[l], sfm_f);
}第五步：三角化其他未恢复的特征点。至此得到了滑动窗口中所有图像帧的位姿以及特征点的 3d 坐标
for (int j = 0; j < feature_num; j++)
{
if (sfm_f[j].state == true)
continue;
if ((int)sfm_f[j].observation.size() >= 2)
{
Vector2d point0, point1;
int frame_0 = sfm_f[j].observation[0].first;
point0 = sfm_f[j].observation[0].second;
int frame_1 = sfm_f[j].observation.back().first;
point1 = sfm_f[j].observation.back().second;
Vector3d point_3d;
triangulatePoint(Pose[frame_0], Pose[frame_1], point0, point1, point_3d);
sfm_f[j].state = true;
sfm_f[j].position[0] = point_3d(0);
sfm_f[j].position[1] = point_3d(1);
sfm_f[j].position[2] = point_3d(2);
//cout << "trangulated : " << frame_0 << " " << frame_1 << " 3d point : " << j << " " << point_3d.transpose() << endl;
}
}第六步：使用 ceres 进行全局 BA 优化
前面六步都通过三角化以及求解PnP的方式，得到一些3d点以及每一帧的位姿，这一步使用ceres对全局进行一次优化，使得所得到的3d点以及位姿更加准确。
在高博的《视觉SLAM十四讲》中，提到PnP中BA问题的求解，这里是个人参考十四讲记录的笔记：ch7_求解PnP中的BA问题，之后可以参考：VSLAM中的非线性优化了解整个非线性优化的过程。
这里需要逐一几个问题：
- ceres中待优化的参数是使用 double数组进行维护的，因此需要将待优化参数转换为 double数组
// 平移
c_translation[i][0] = c_Translation[i].x();
c_translation[i][1] = c_Translation[i].y();
c_translation[i][2] = c_Translation[i].z();
// 旋转四元数
c_rotation[i][0] = c_Quat[i].w();
c_rotation[i][1] = c_Quat[i].x();
c_rotation[i][2] = c_Quat[i].y();
c_rotation[i][3] = c_Quat[i].z();- 四元数的加法是广义的加法，因此需要进行局部参数化
// 局部参数化，定义四元数的特殊加法
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
problem.AddParameterBlock(c_rotation[i], 4, local_parameterization); // 这里使用local_parameterization声明这是旋转四元数的广义的加法
problem.AddParameterBlock(c_translation[i], 3);- 由于视觉SLAM存在零空间漂移的问题，因此需要 fix 一些帧吗，这里 fix 的是参考帧的旋转和平移，以及当前帧的平移
// 由于时单目视觉SLAM，有七个自由度不可观，因此，fix（固定）一些参数块，避免在零空间出现漂移
// fix设置的世界坐标系地l帧的位姿，同时fix最后一帧的位移用来fix尺度
if (i == l)
{
problem.SetParameterBlockConstant(c_rotation[i]); // 参考帧l的旋转不参与优化
}
if (i == l || i == frame_num - 1)
{
problem.SetParameterBlockConstant(c_translation[i]);// 参考帧平移以及最后一帧的旋转不参与优化
}- 构建BA的时候，由于都存在视觉重投影误差约束，因此可以直接遍历所有路标点
// 只有视觉重投影构成的约束，因此遍历所有的特征点，构建约束
for (int i = 0; i < feature_num; i++) // 遍历所有的路标点
{
if (sfm_f[i].state != true) // 必须经过三角化
continue;
// 遍历所有观测帧，对这些帧建立约束
for (int j = 0; j < int(sfm_f[i].observation.size()); j++) //每一个路标点被观测的所有帧
{
int l = sfm_f[i].observation[j].first; // 路标点被观测帧的id
ceres::CostFunction* cost_function = ReprojectionError3D::Create(
sfm_f[i].observation[j].second.x(), //路标点被观测帧的归一化坐标
sfm_f[i].observation[j].second.y());
// 约束了这一帧位姿和3d坐标点
// AddResidualBlock: 代价函数模块、损失函数模块和参数模块
problem.AddResidualBlock(cost_function, NULL, c_rotation[l], c_translation[l],
sfm_f[i].position);
}
}- 这里的优化使用的是ceres的自动求导，自动求导要求使用者自定义一个仿函数，其实是一个类，只不过这个类的作用像函数，所以叫仿函数。原理就是类实现了
operator()函数。该函数的作用就是告诉ceres怎么求，然后ceres根据用户需求求出最终的雅可比。
struct ReprojectionError3D
{
ReprojectionError3D(double observed_u, double observed_v)
: observed_u(observed_u), observed_v(observed_v)
{
}
// 定义仿函数
template <typename T>
bool operator()(const T *const camera_R, const T *const camera_T, const T *point, T *residuals) const
{
T p[3];
ceres::QuaternionRotatePoint(camera_R, point, p); // 旋转这个点
// Rcw * Pw + tcw
p[0] += camera_T[0];
p[1] += camera_T[1];
p[2] += camera_T[2];
// 归一化处理
T xp = p[0] / p[2];
T yp = p[1] / p[2];
// 跟现在的观测形成残差
residuals[0] = xp - T(observed_u);
residuals[1] = yp - T(observed_v);
return true;
}
// 构造CostFunction
static ceres::CostFunction *Create(const double observed_x,
const double observed_y)
{
return (new ceres::AutoDiffCostFunction<
ReprojectionError3D, 2, 4, 3, 3>(
new ReprojectionError3D(observed_x, observed_y)));
}
double observed_u;
double observed_v;
}; AutoDiffCostFunction的模板参数：
- 第1个参数是仿函数
ReprojectionError3D - 第2个参数是残差块中残差的数量，这里指的就是归一化坐标
- 第3个参数是第一个参数块中参数的数量，
4表示camera_R的长度，3表示位移camera_T的长度，3表示 3D点的长度
- 进行ceres 自动求导
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
//options.minimizer_progress_to_stdout = true;
options.max_solver_time_in_seconds = 0.2; // 最大求解时间
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);四、对所有图像帧进行PnP
通过三维重建的操作，得到了窗口中所有图像帧的位姿 QT（相对于第 l 帧）和特征点坐标 sfm_tracked_points。至此，对滑窗中的关键帧的位姿以及3D点坐标进行了求解，但是，初始化由于需要所有帧的参与，因此还需要对所有帧all_image_frame（包括非关键帧）进行PnP 的求取，求取的方式就是根据之前求取的关键帧的位姿来得到。
// 下面通过KF来求解其他的非KF的位姿
map<double, ImageFrame>::iterator frame_it;
map<int, Vector3d>::iterator it;
frame_it = all_image_frame.begin();
// i代表跟这个帧最近的KF索引
for (int i = 0; frame_it != all_image_frame.end(); frame_it++) // 遍历所有帧
{
// provide initial guess
cv::Mat r, rvec, t, D, tmp_r;
// 如果时间戳一致,该帧本来就是KF,因此可以直接得到位姿
if ((frame_it->first) == Headers[i].stamp.toSec())
{
frame_it->second.is_key_frame = true;// 是否是关键帧标志位职true
frame_it->second.R = Q[i].toRotationMatrix() * RIC[0].transpose(); // 得到Rwi
frame_it->second.T = T[i]; // 初始化不顾及平移外参
i++;
continue;
}
// 如果时间戳大于关键帧,则寻找下一个关键帧，即非关键帧使用时间戳小于自己的关键帧的位姿
if ((frame_it->first) > Headers[i].stamp.toSec())
{
i++;
}
// Twc->Tcw
// 注意这里的 Q和 T是图像帧的位姿，而不是求解PNP时所用的坐标系变换矩阵，两者具有对称关系
Matrix3d R_inital = (Q[i].inverse()).toRotationMatrix();
Vector3d P_inital = -R_inital * T[i];
cv::eigen2cv(R_inital, tmp_r);
cv::Rodrigues(tmp_r, rvec);//罗德里格斯公式将旋转矩阵转换成旋转向量
cv::eigen2cv(P_inital, t);
frame_it->second.is_key_frame = false;
//获取 pnp需要用到的存储每个特征点三维点和图像坐标的 vector
vector<cv::Point3f> pts_3_vector;
vector<cv::Point2f> pts_2_vector;
for (auto &id_pts : frame_it->second.points)
{
int feature_id = id_pts.first;
for (auto &i_p : id_pts.second)
{
it = sfm_tracked_points.find(feature_id);
if (it != sfm_tracked_points.end())
{
// 取出3D点
Vector3d world_pts = it->second;
cv::Point3f pts_3(world_pts(0), world_pts(1), world_pts(2));
pts_3_vector.push_back(pts_3);
// 取出2D点
Vector2d img_pts = i_p.second.head<2>();
cv::Point2f pts_2(img_pts(0), img_pts(1));
pts_2_vector.push_back(pts_2);
}
}
}
//保证特征点数大于 5
cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
if (pts_3_vector.size() < 6)
{
cout << "pts_3_vector size " << pts_3_vector.size() << endl;
ROS_DEBUG("Not enough points for solve pnp !");
return false;
}
/**
*bool cv::solvePnP( 求解 pnp问题
* InputArray objectPoints, 特征点的3D坐标数组
* InputArray imagePoints, 特征点对应的图像坐标
* InputArray cameraMatrix, 相机内参矩阵
* InputArray distCoeffs, 失真系数的输入向量
* OutputArray rvec, 旋转向量
* OutputArray tvec, 平移向量
* bool useExtrinsicGuess = false, 为真则使用提供的初始估计值
* int flags = SOLVEPNP_ITERATIVE 采用LM优化
*)
*/
if (!cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1))
{
ROS_DEBUG("solve pnp fail!");
return false;
}
// cv->eigen 同时Tcw -> Twc
cv::Rodrigues(rvec, r);
MatrixXd R_pnp, tmp_R_pnp;
cv::cv2eigen(r, tmp_R_pnp);
// Twc -> Twi
//这里也同样需要将坐标变换矩阵转变成图像帧位姿，并转换为IMU坐标系的位姿
R_pnp = tmp_R_pnp.transpose();
MatrixXd T_pnp;
cv::cv2eigen(t, T_pnp);
T_pnp = R_pnp * (-T_pnp);
frame_it->second.R = R_pnp * RIC[0].transpose();
frame_it->second.T = T_pnp;
}
3 条评论
用的handsome