双目vio视觉前端以及后端重投影残差(vins双目)
本次主要讲解vio双目系统的视觉前端部分和后端融合的重投影残差部分,以vins切入做为分析,并赋代码注释解析。
主要包含以下五部分内容
1)双目特征点提取;
2)特征点光流跟踪;
3)特征点三角化;
4)pnp求解相机帧间运动;
5)后端优化之视觉特征点重投影残差Bundle Adjustment(BA)
1. 特征提取
1.1 左右目时间对齐
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> 7个数据分别代表:
归一化坐标:x, y, z(z=1)
像素坐标:p_u, p_v
帧间像素移动速度: velocity_x, velocity_y;
1.2 特征点检测
通过调用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角点检测时使用
);
2. 特征点光流跟踪
函数:featureFrame = featureTracker.trackImage(t, _img, _img1);
2.1 光流跟踪
光流跟踪:
光流法主要用于寻找不同图片间的特征点对应关系,表示每个像素在相邻帧间相对位置的位移向量。Lucas-Kanade 稀疏光流法只依赖于围绕关键点的小窗口推断出的局部信息。这导致了Lucas-Kanade算法不能检测到物体的快速运动到窗口外部的点。这个缺点可以通过改进的金字塔LK光流法解决。
vins中使用金字塔LK光流法:由于像素运动过大可能导致不满足光流法的窗口假设,无法跟踪到对应的特征点,所以采用光流金字塔:
第0层为原始图像,每一层向上对图像像素宽高缩小两倍,呈金字塔形状。首先计算最高层的光流大小,做为初始值计算次高层的光流大小…以此类推到原始图像即可。
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,表示使用两层金字塔。
2.2 vins中对应特征点提取跟踪代码流程解析
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);
}
2)在第一帧图像的情况下:
首先对左目图像提取150个特征点,并存储特征点到cur_pts中:
在mask值为0处不进行角点检测,所以第一次设置图像mask并没有起作用。
cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask);//且在mask值为0处不进行角点检测。
接下来对检测出来的角点进行去畸变并求出像素运动速度(第一帧速度为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;
3)在不是第一帧图像的情况下:
首先进行前后两帧特征点跟踪(针对左目,右目前后两帧不需要),并且进行反向光流跟踪。
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>是归一化坐标,像素坐标,归一化坐标系下像素运动速度。
3. 特征点三角化
之所以把三角化放在前面,是因为相机第一帧的位置由imu初始化得到,并且后续的帧间pnp算法恢复帧间运动需要用到特征点在 world 系的位置,所以当第一帧图像来的时候可以根据左右目的特征跟踪,来三角化特征点。当之后的特征点被多帧图像跟踪到时,采用最小二乘进行三角化。
在vins中,并没有采用相似原理去获得特征点深度,使用方法如下:
注意:代码中的T是代表 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)求解。不会执行这里。
4. PNP求解相机帧间运动
这里不再阐述pnp具体算法原理,其原理类似于重投影BA的过程,证明方法可参考视觉slam 14 讲P.157
代码中调用opencv函数进行求解:
cv::solvePnP(pts3D, pts2D, K, D, rvec, t, 1);
注意参数的使用,尤其是rvec和t的使用,都是从world系到cam系,参考。。。
5. 视觉重投影残差(属于后端优化部分)
视觉特征点重投影残差Bundle Adjustment(BA)原理:
在相机归一化平面上,将残差定义为:E = 特征点在当前帧中光流跟踪的归一化坐标 - 特征点的在该帧的重投影归一化坐标。
Jacobian矩阵为视觉残差对两个时刻的状态变量,imu 和 camera 外参,以及逆深度求导;
使用链式求导法则,如下:
Jacobian 矩阵的具体形式略,可以参照vins代码自行推导;
重投影的残差需要区分图像是否为第一帧;
1)当图像为第一帧时,需要左右目做重投影残差;
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;
}
2)当图像不是第一帧时,需要划窗内特征点 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 求解即可!