本次主要讲解vio双目系统的视觉前端部分和后端融合的重投影残差部分，以vins切入做为分析，并赋代码注释解析。
主要包含以下五部分内容
１）双目特征点提取；
２）特征点光流跟踪；
３）特征点三角化；
４）pnp求解相机帧间运动；
５）后端优化之视觉特征点重投影残差Bundle Adjustment(BA)
imu回调函数：void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)；
将收到的数据存储到相应的buffer中，accBuf， gyrBuf
图像回调函数：
void img0_callback(const sensor_msgs::ImageConstPtr &img_msg)－>左目
void img1_callback(const sensor_msgs::ImageConstPtr &img_msg)－>左目
时间对齐后存储图像数据
特征点id 相机id
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
其中Matrix<double, 7, 1> ７个数据分别代表：
归一化坐标：x, y, z（z=1）
像素坐标：p_u, p_v
帧间像素移动速度： velocity_x, velocity_y;
通过调用opencv库函数进行角点检测cv::goodFeaturesToTrack()，并保证每一帧图像有150（150是可调参数）个特征点，当光流跟踪少于150个，就会运行该函数提取特征补充到150个。
cv::goodFeaturesToTrack()，它不仅支持Harris角点检测，也支持Shi Tomasi算法的角点检测。但是，该函数检测到的角点依然是像素级别的，若想获取更为精细的角点坐标，则需要调用cv::cornerSubPix()函数进一步细化处理，即亚像素。
void cv::goodFeaturesToTrack(
cv::InputArray image, // 输入图像（CV_8UC1 CV_32FC1）
cv::OutputArray corners, // 输出角点vector
int maxCorners, // 最大角点数目
double qualityLevel, // 质量水平系数（小于1.0的正数，一般在0.01-0.1之间）
double minDistance, // 最小距离，小于此距离的点忽略
cv::InputArray mask = noArray(), // mask=0的点忽略
int blockSize = 3, // 使用的邻域数
bool useHarrisDetector = false, // false ='Shi Tomasi metric'
double k = 0.04 // Harris角点检测时使用
);
函数：featureFrame = featureTracker.trackImage(t, _img, _img1);
光流跟踪：
光流法主要用于寻找不同图片间的特征点对应关系，表示每个像素在相邻帧间相对位置的位移向量。Lucas-Kanade 稀疏光流法只依赖于围绕关键点的小窗口推断出的局部信息。这导致了Lucas-Kanade算法不能检测到物体的快速运动到窗口外部的点。这个缺点可以通过改进的金字塔LK光流法解决。
vins中使用金字塔LK光流法：由于像素运动过大可能导致不满足光流法的窗口假设，无法跟踪到对应的特征点，所以采用光流金字塔：
第０层为原始图像，每一层向上对图像像素宽高缩小两倍，呈金字塔形状。首先计算最高层的光流大小，做为初始值计算次高层的光流大小…以此类推到原始图像即可。
cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 1,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
vins中Level＝１，表示使用两层金字塔。
1)首先时间对齐后从 image_buffer 图像取出，重新分布亮度来来改变图像对比度（只适用于灰度图像）。这样能更好的检测出角点。
// 自适应直方图均衡(CLAHE)
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(16, 16));
clahe->apply(cur_img, cur_img);
if(!rightImg.empty())
clahe->apply(rightImg, rightImg);
}
２)在第一帧图像的情况下：
首先对左目图像提取150个特征点，并存储特征点到cur_pts中：
在mask值为0处不进行角点检测，所以第一次设置图像mask并没有起作用。
cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask);//且在mask值为0处不进行角点检测。
接下来对检测出来的角点进行去畸变并求出像素运动速度（第一帧速度为０）：
cur_un_pts = undistortedPts(cur_pts, m_camera[0]);//去畸变
pts_velocity = ptsVelocity(ids, cur_un_pts, cur_un_pts_map, prev_un_pts_map);//计算像素速度
右目对左目的图像进行光流跟踪，并存储特征点到cur_right_pts中：
cv::calcOpticalFlowPyrLK(cur_img, rightImg, cur_pts, cur_right_pts, status, err, cv::Size(21, 21), 3);
然后使用反向光流跟踪，即左目再跟踪右目的特征点，如果两次跟踪特征点均存在，则得以保留，否则将在cur_pts，cur_right_pts中删除掉。
最后将特征点保存到数据结构中map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
３)在不是第一帧图像的情况下：
首先进行前后两帧特征点跟踪（针对左目，右目前后两帧不需要），并且进行反向光流跟踪。
cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);
// reverse check
if(FLOW_BACK)
{
vector<uchar> reverse_status;
vector<cv::Point2f> reverse_pts = prev_pts;
cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 1,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
//cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 3);
for(size_t i = 0; i < status.size(); i++)
{
if(status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5)
{
status[i] = 1;
}
else
status[i] = 0;
}
}
当跟踪到的特征点数特征点少于设定值时，进行特征点检测补充到特定值：
根据第一帧图像设置的掩模（mask），在特征点黑色区域内不再进行特征点重复检测，不仅提高算法执行速度，而且能够使得特征点尽量均匀分布。
if(inputImageCnt % 2 == 0)
{
mBuf.lock();
featureBuf.push(make_pair(t, featureFrame));
mBuf.unlock();
}
queue<pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > > featureBuf;
最后将提取的特征点，放到 featureBuf 中；
结构体从左到右分别是时间戳，特征点id，相机id，
Matrix<double, 7, 1>是归一化坐标，像素坐标，归一化坐标系下像素运动速度。
之所以把三角化放在前面，是因为相机第一帧的位置由imu初始化得到，并且后续的帧间pnp算法恢复帧间运动需要用到特征点在 world 系的位置，所以当第一帧图像来的时候可以根据左右目的特征跟踪，来三角化特征点。当之后的特征点被多帧图像跟踪到时，采用最小二乘进行三角化。
在vins中，并没有采用相似原理去获得特征点深度，使用方法如下：
注意：代码中的Ｔ是代表 world 系到 camera 系，所以需要取逆。
对应代码如下：
void FeatureManager::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
Eigen::Vector2d &point0, Eigen::Vector2d &point1, Eigen::Vector3d &point_3d)
{
Eigen::Matrix4d design_matrix = Eigen::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);
Eigen::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);
}
最后通过svd求解。
当右目没有跟踪到左目的特征点时，此时需要前后两帧之间进行三角化，原理同上。
代码中还有一部分冗余，当特征点跟踪次数大于4时，此时三角化方程为超定方程，使用超定方程 最小二乘解 奇异值分解（SVD）求解。不会执行这里。
这里不再阐述pnp具体算法原理，其原理类似于重投影ＢＡ的过程，证明方法可参考视觉slam 14 讲P.157
代码中调用opencv函数进行求解：
cv::solvePnP(pts3D, pts2D, K, D, rvec, t, 1);
注意参数的使用，尤其是ｒｖｅｃ和ｔ的使用，都是从world系到cam系，参考。。。
视觉特征点重投影残差Bundle Adjustment(BA)原理：
在相机归一化平面上，将残差定义为：E = 特征点在当前帧中光流跟踪的归一化坐标 － 特征点的在该帧的重投影归一化坐标。
Jacobian矩阵为视觉残差对两个时刻的状态变量，imu 和 camera 外参，以及逆深度求导；
使用链式求导法则，如下：
Jacobian 矩阵的具体形式略，可以参照vins代码自行推导；
重投影的残差需要区分图像是否为第一帧；
１）当图像为第一帧时，需要左右目做重投影残差；
bool ProjectionOneFrameTwoCamFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
TicToc tic_toc;
Eigen::Vector3d tic(parameters[0][0], parameters[0][1], parameters[0][2]);
Eigen::Quaterniond qic(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
Eigen::Vector3d tic2(parameters[1][0], parameters[1][1], parameters[1][2]);
Eigen::Quaterniond qic2(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);
double inv_dep_i = parameters[2][0];
double td = parameters[3][0];
Eigen::Vector3d pts_i_td, pts_j_td;
pts_i_td = pts_i - (td - td_i) * velocity_i;
pts_j_td = pts_j - (td - td_j) * velocity_j;
Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i;
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
Eigen::Vector3d pts_imu_j = pts_imu_i;
Eigen::Vector3d pts_camera_j = qic2.inverse() * (pts_imu_j - tic2);
Eigen::Map<Eigen::Vector2d> residual(residuals);
double dep_j = pts_camera_j.z();
residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>();
residual = sqrt_info * residual;
if (jacobians)
{
Eigen::Matrix3d ric = qic.toRotationMatrix();
Eigen::Matrix3d ric2 = qic2.toRotationMatrix();
Eigen::Matrix<double, 2, 3> reduce(2, 3);
reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j),
0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j);
reduce = sqrt_info * reduce;
Eigen::Matrix4d tmp_M = -Eigen::Matrix4d::Identity();
tmp_M(3, 3) = 1;
if (jacobians[0])
{
Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_ex_pose(jacobians[0]);
Eigen::Matrix<double, 3, 7> jaco_ex;
jaco_ex.leftCols<3>() = ric2.transpose();
jaco_ex.rightCols<4>() = ric2.transpose() * Utility::QuaternionDerivation(qic, pts_camera_i);
jacobian_ex_pose = reduce * jaco_ex;
}
if (jacobians[1])
{
Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_ex_pose1(jacobians[1]);
Eigen::Matrix<double, 3, 7> jaco_ex;
jaco_ex.leftCols<3>() = -ric2.transpose();
jaco_ex.rightCols<4>() = Utility::QuaternionDerivation(qic2.inverse(), pts_imu_j-tic2) * tmp_M;
jacobian_ex_pose1 = reduce * jaco_ex;
}
if (jacobians[2])
{
Eigen::Map<Eigen::Vector2d> jacobian_feature(jacobians[2]);
jacobian_feature = reduce * ric2.transpose() * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i);
}
if (jacobians[3])
{
Eigen::Map<Eigen::Vector2d> jacobian_td(jacobians[3]);
jacobian_td = reduce * ric2.transpose() * ric * velocity_i / inv_dep_i * -1.0 +
sqrt_info * velocity_j.head(2);
}
}
sum_t += tic_toc.toc();
return true;
}
２）当图像不是第一帧时，需要划窗内特征点 start_frame 和 current_frame 做重投影残差
bool ProjectionTwoFrameTwoCamFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
TicToc tic_toc;
Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);
Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);
Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);
Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);
Eigen::Vector3d tic2(parameters[3][0], parameters[3][1], parameters[3][2]);
Eigen::Quaterniond qic2(parameters[3][6], parameters[3][3], parameters[3][4], parameters[3][5]);
double inv_dep_i = parameters[4][0];
double td = parameters[5][0];
Eigen::Vector3d pts_i_td, pts_j_td;
pts_i_td = pts_i - (td - td_i) * velocity_i;
pts_j_td = pts_j - (td - td_j) * velocity_j;
Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i;
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
Eigen::Vector3d pts_camera_j = qic2.inverse() * (pts_imu_j - tic2);
Eigen::Map<Eigen::Vector2d> residual(residuals);
double dep_j = pts_camera_j.z();
residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>();
residual = sqrt_info * residual;
if (jacobians)
{
Eigen::Matrix3d Ri = Qi.toRotationMatrix();
Eigen::Matrix3d Rj = Qj.toRotationMatrix();
Eigen::Matrix3d ric = qic.toRotationMatrix();
Eigen::Matrix3d ric2 = qic2.toRotationMatrix();
Eigen::Matrix<double, 2, 3> reduce(2, 3);
reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j),
0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j);
reduce = sqrt_info * reduce;
Eigen::Matrix4d tmp_M = -Eigen::Matrix4d::Identity();
tmp_M(3, 3) = 1;
if (jacobians[0])
{
Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
Eigen::Matrix<double, 3, 7> jaco_i;
jaco_i.leftCols<3>() = ric2.transpose() * Rj.transpose();
jaco_i.rightCols<4>() = ric2.transpose() * Rj.transpose() * Utility::QuaternionDerivation(Qi, pts_imu_i);
jacobian_pose_i = reduce * jaco_i;
}
if (jacobians[1])
{
Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[1]);
Eigen::Matrix<double, 3, 7> jaco_j;
jaco_j.leftCols<3>() = ric2.transpose() * -Rj.transpose();
jaco_j.rightCols<4>() = ric2.transpose() * Utility::QuaternionDerivation(Qj.inverse(), pts_w-Pj) * tmp_M;
jacobian_pose_j = reduce * jaco_j;
}
if (jacobians[2])
{
Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_ex_pose(jacobians[2]);
Eigen::Matrix<double, 3, 7> jaco_ex;
Eigen::Matrix3d tmp_R = ric2.transpose() * Rj.transpose() * Ri;
jaco_ex.leftCols<3>() = tmp_R;
jaco_ex.rightCols<4>() = tmp_R * Utility::QuaternionDerivation(qic, pts_camera_i);
jacobian_ex_pose = reduce * jaco_ex;
}
if (jacobians[3])
{
Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_ex_pose1(jacobians[3]);
Eigen::Matrix<double, 3, 7> jaco_ex;
jaco_ex.leftCols<3>() = -ric2.transpose();
jaco_ex.rightCols<4>() = Utility::QuaternionDerivation(qic2.inverse(), pts_imu_j-tic2) * tmp_M;
jacobian_ex_pose1 = reduce * jaco_ex;
}
if (jacobians[4])
{
Eigen::Map<Eigen::Vector2d> jacobian_feature(jacobians[4]);
jacobian_feature = reduce * ric2.transpose() * Rj.transpose() * Ri * ric * pts_i_td * -1.0 / (inv_dep_i * inv_dep_i);
}
if (jacobians[5])
{
Eigen::Map<Eigen::Vector2d> jacobian_td(jacobians[5]);
jacobian_td = reduce * ric2.transpose() * Rj.transpose() * Ri * ric * velocity_i / inv_dep_i * -1.0 +
sqrt_info * velocity_j.head(2);
}
}
sum_t += tic_toc.toc();
return true;
}
使用 ceres 求解即可！