msckf_image_processor_integrateimudata-CSDN博客

本文链接：https://blog.csdn.net/fb_941219/article/details/124987403

msckf_image_processor

0.引言

文章只是自己过一遍代码的笔记，大部分参考自网络，尽量都列出出处。

主要参考博客

1.ProcessorConfig

请添加图片描述

2.主要流程

主要逻辑在 stereoCallback 函数中

请添加图片描述

3.初始化

请添加图片描述

3.1.createImagePyramids

调用OpenCV函数对左右目的图像构建金字塔：

  const Mat& curr_cam0_img = cam0_curr_img_ptr->image;
  buildOpticalFlowPyramid(
      curr_cam0_img, curr_cam0_pyramid_,
      Size(processor_config.patch_size, processor_config.patch_size),
      processor_config.pyramid_levels, true, BORDER_REFLECT_101,
      BORDER_CONSTANT, false);

  // Pyramids for previous and current image
  std::vector<cv::Mat> prev_cam0_pyramid_;
  std::vector<cv::Mat> curr_cam0_pyramid_;
  std::vector<cv::Mat> curr_cam1_pyramid_;

参考博客

int cv::buildOpticalFlowPyramid (InputArray img,OutputArrayOfArrays pyramid,Size winSize,int maxLevel,bool withDerivatives = true,int pyrBorder = BORDER_REFLECT_101,int derivBorder = BORDER_CONSTANT,bool tryReuseInputImage = true)

参数名称	参数描述
img	8位输入图像 curr_cam0_img
pyramid	输出金字塔 std::vectorcv::Mat curr_cam0_pyramid_;
winSize	光流算法的窗口大小。必须不少于calcOpticalFlowPyrLK的winSize参数。需要计算金字塔级别所需的填充。31*31
maxLevel	从0开始的最大金字塔等级编号。3
withDerivatives	设置为每个金字塔等级预计算梯度。如果金字塔是在没有梯度的情况下构建的，那么calcOpticalFlowPyrLK将在内部对其进行计算。
pyrBorder	金字塔图层的边框模式。
derivBorder	梯度边框模式。
tryReuseInputImage	如果可能，将输入图像的ROI放入金字塔中。您可以传递false来强制复制数据。

参考博客

3.2.初始化第一帧

1.initializeFirstFrame()

请添加图片描述

stereoMatch
- 预处理:cam0_points按照cam0的参数去畸变,再按照cam1的参数进行畸变处理作为cam1_points的初始值
- Step1: 将cam0的点光流跟踪到cam1
- Step2: 去除跟踪到图像外的点
- Step3: 根据左右目相机外参计算E矩阵
- Step4: 对左右目feature分别去畸变,利用E矩阵进行outlier判断

Step1: 将cam0的点光流跟踪到cam1, calcOpticalFlowPyrLk

void cv::calcOpticalFlowPyrLK(	InputArray 	prevImg,
								InputArray 	nextImg,
								InputArray 	prevPts,
								InputOutputArray 	nextPts,
								OutputArray 	status,
								OutputArray 	err,
								Size 	winSize = Size(21, 21),
								int 	maxLevel = 3,
								TermCriteria 	criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01),
								int 	flags = 0,
								double 	minEigThreshold = 1e-4 
)

参数名称	参数描述
prevImg	buildOpticalFlowPyramid构造的第一个8位输入图像或金字塔。
nextImg	与prevImg相同大小和相同类型的第二个输入图像或金字塔
prevPts	需要找到流的2D点的矢量(vector of 2D points for which the flow needs to be found;);点坐标必须是单精度浮点数。
nextPts	输出二维点的矢量（具有单精度浮点坐标），包含第二图像中输入特征的计算新位置;当传递OPTFLOW_USE_INITIAL_FLOW标志时，向量必须与输入中的大小相同。
status	输出状态向量（无符号字符）;如果找到相应特征的流，则向量的每个元素设置为1，否则设置为0。
err	输出错误的矢量; 向量的每个元素都设置为相应特征的错误，错误度量的类型可以在flags参数中设置; 如果未找到流，则未定义错误（使用status参数查找此类情况）。
winSize	每个金字塔等级的搜索窗口的winSize大小。
maxLevel	基于0的最大金字塔等级数;如果设置为0，则不使用金字塔（单级），如果设置为1，则使用两个级别，依此类推;如果将金字塔传递给输入，那么算法将使用与金字塔一样多的级别，但不超过maxLevel。
criteria	参数，指定迭代搜索算法的终止条件（在指定的最大迭代次数criteria.maxCount之后或当搜索窗口移动小于criteria.epsilon时）。
flags	操作标志：OPTFLOW_USE_INITIAL_FLOW使用初始估计，存储在nextPts中;如果未设置标志，则将prevPts复制到nextPts并将其视为初始估计。OPTFLOW_LK_GET_MIN_EIGENVALS使用最小特征值作为误差测量（参见minEigThreshold描述）;如果没有设置标志，则将原稿周围的色块和移动点之间的L1距离除以窗口中的像素数，用作误差测量。
minEigThreshold	算法计算光流方程的2x2正常矩阵的最小特征值，除以窗口中的像素数;如果此值小于minEigThreshold，则过滤掉相应的功能并且不处理其流程，因此它允许删除坏点并获得性能提升。

  // 将cam0的点光流跟踪到cam1
  calcOpticalFlowPyrLK(curr_cam0_pyramid_, curr_cam1_pyramid_,
      cam0_points, cam1_points,
      inlier_markers, noArray(),
      Size(processor_config.patch_size, processor_config.patch_size),
      processor_config.pyramid_levels,
      TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,
                   processor_config.max_iteration,
                   processor_config.track_precision),
      cv::OPTFLOW_USE_INITIAL_FLOW);

Step3: 根据左右目相机外参（标定所得）计算E矩阵：

  // 根据左右目相机外参计算E矩阵
  const cv::Matx33d R_cam0_cam1 = R_cam1_imu.t() * R_cam0_imu;
  const cv::Vec3d t_cam0_cam1 = R_cam1_imu.t() * (t_cam0_imu-t_cam1_imu);
  // Compute the essential matrix.
  const cv::Matx33d t_cam0_cam1_hat(
      0.0, -t_cam0_cam1[2], t_cam0_cam1[1],
      t_cam0_cam1[2], 0.0, -t_cam0_cam1[0],
      -t_cam0_cam1[1], t_cam0_cam1[0], 0.0);
  // 对极几何
  const cv::Matx33d E = t_cam0_cam1_hat * R_cam0_cam1;

Step4: 对左右目feature分别去畸变,利用E矩阵进行outlier判断

vector<cv::Point2f> cam0_points_undistorted(0);
vector<cv::Point2f> cam1_points_undistorted(0);
undistortPoints(
  cam0_points, cam0_intrinsics, cam0_distortion_model,
  cam0_distortion_coeffs, cam0_points_undistorted);
undistortPoints(
  cam1_points, cam1_intrinsics, cam1_distortion_model,
  cam1_distortion_coeffs, cam1_points_undistorted);
// 1 / f
double norm_pixel_unit = 4.0 / (
  cam0_intrinsics[0]+cam0_intrinsics[1]+
  cam1_intrinsics[0]+cam1_intrinsics[1]);

for (int i = 0; i < cam0_points_undistorted.size(); ++i) {
if (inlier_markers[i] == 0) continue;
// 归一化平面坐标系
cv::Vec3d pt0(cam0_points_undistorted[i].x,
    cam0_points_undistorted[i].y, 1.0);
cv::Vec3d pt1(cam1_points_undistorted[i].x,
    cam1_points_undistorted[i].y, 1.0);
// 极线
cv::Vec3d epipolar_line = E * pt0;
// 点到直线的距离
double error = fabs((pt1.t() * epipolar_line)[0]) / sqrt(
    epipolar_line[0]*epipolar_line[0]+
    epipolar_line[1]*epipolar_line[1]);
// 误差过大则设置为外点
if (error > processor_config.stereo_threshold*norm_pixel_unit)
  inlier_markers[i] = 0;
}

4.trackFeatures

当第一帧初始化完成之后,后面帧则只需要进行跟踪第一帧的特征点,并且提取新的特征点,整个流程如下:
请添加图片描述

请添加图片描述

4.1.计算前后帧imu相对旋转integrateImuData

  // Compute the mean angular velocity in the IMU frame.
  // 计算平均角速度
  Vec3f mean_ang_vel(0.0, 0.0, 0.0);
  for (auto iter = begin_iter; iter < end_iter; ++iter)
    mean_ang_vel += Vec3f(iter->angular_velocity.x,
        iter->angular_velocity.y, iter->angular_velocity.z);

  if (end_iter-begin_iter > 0)
    mean_ang_vel *= 1.0f / (end_iter-begin_iter);

  // Transform the mean angular velocity from the IMU
  // frame to the cam0 and cam1 frames.
  // 平均角速度转到 cam0 和 cam1 坐标系中  t()转置
  // ? R_cam0_imu 代表 cam0 2 imu
  Vec3f cam0_mean_ang_vel = R_cam0_imu.t() * mean_ang_vel;
  Vec3f cam1_mean_ang_vel = R_cam1_imu.t() * mean_ang_vel;

  // Compute the relative rotation.
  // delta t
  double dtime = (cam0_curr_img_ptr->header.stamp-
      cam0_prev_img_ptr->header.stamp).toSec();
  // 调用 OpenCV 的罗德里格斯公式将旋转向量转为旋转矩阵
  Rodrigues(cam0_mean_ang_vel*dtime, cam0_R_p_c);
  Rodrigues(cam1_mean_ang_vel*dtime, cam1_R_p_c);
  // prev 2 curr
  cam0_R_p_c = cam0_R_p_c.t();
  cam1_R_p_c = cam1_R_p_c.t();

4.2.根据相机相对旋转预测cam0上一帧特征点在当前帧中的位置predictFeatureTracking

假设为纯旋转，单应矩阵则为：

$H = K R_{pre}^{cur} K ^{-1}$ $x_{cur} = H_{pre}^{cur} x_{pre}$

  // Intrinsic matrix.
  cv::Matx33f K(
      intrinsics[0], 0.0, intrinsics[2],
      0.0, intrinsics[1], intrinsics[3],
      0.0, 0.0, 1.0);
  // 计算单应矩阵
  cv::Matx33f H = K * R_p_c * K.inv();

  for (int i = 0; i < input_pts.size(); ++i) {
    cv::Vec3f p1(input_pts[i].x, input_pts[i].y, 1.0f);
    // 预测位置
    cv::Vec3f p2 = H * p1;
    // 归一化处理
    compensated_pts[i].x = p2[0] / p2[2];
    compensated_pts[i].y = p2[1] / p2[2];
  }

4.3.cam0前后帧光流跟踪

  calcOpticalFlowPyrLK(
      prev_cam0_pyramid_, curr_cam0_pyramid_,
      prev_cam0_points, curr_cam0_points,
      track_inliers, noArray(),
      Size(processor_config.patch_size, processor_config.patch_size),
      processor_config.pyramid_levels,
      TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,
        processor_config.max_iteration,
        processor_config.track_precision),
      cv::OPTFLOW_USE_INITIAL_FLOW);

参数名称	参数描述
prevImg	buildOpticalFlowPyramid构造的第一个8位输入图像或金字塔。
nextImg	与prevImg相同大小和相同类型的第二个输入图像或金字塔
prevPts	需要找到流的2D点的矢量(vector of 2D points for which the flow needs to be found;);点坐标必须是单精度浮点数。
nextPts	输出二维点的矢量（具有单精度浮点坐标），包含第二图像中输入特征的计算新位置;当传递OPTFLOW_USE_INITIAL_FLOW标志时，向量必须与输入中的大小相同。
status	输出状态向量（无符号字符）;如果找到相应特征的流，则向量的每个元素设置为1，否则设置为0。
err	输出错误的矢量; 向量的每个元素都设置为相应特征的错误，错误度量的类型可以在flags参数中设置; 如果未找到流，则未定义错误（使用status参数查找此类情况）。
winSize	每个金字塔等级的搜索窗口的winSize大小。
maxLevel	基于0的最大金字塔等级数;如果设置为0，则不使用金字塔（单级），如果设置为1，则使用两个级别，依此类推;如果将金字塔传递给输入，那么算法将使用与金字塔一样多的级别，但不超过maxLevel。
criteria	参数，指定迭代搜索算法的终止条件（在指定的最大迭代次数criteria.maxCount之后或当搜索窗口移动小于criteria.epsilon时）。
flags	操作标志：OPTFLOW_USE_INITIAL_FLOW使用初始估计，存储在nextPts中;如果未设置标志，则将prevPts复制到nextPts并将其视为初始估计。OPTFLOW_LK_GET_MIN_EIGENVALS使用最小特征值作为误差测量（参见minEigThreshold描述）;如果没有设置标志，则将原稿周围的色块和移动点之间的L1距离除以窗口中的像素数，用作误差测量。
minEigThreshold	算法计算光流方程的2x2正常矩阵的最小特征值，除以窗口中的像素数;如果此值小于minEigThreshold，则过滤掉相应的功能并且不处理其流程，因此它允许删除坏点并获得性能提升。

4.5.对cam0和cam1分别进行前后帧2点法ransac

请添加图片描述

RANSAC(RAndom SAmple Consensus,随机采样一致)算法是从一组含有“外点”(outliers)的数据中正确估计数学模型参数的迭代算法。“外点”一般指的的数据中的噪声，比如说匹配中的误匹配和估计曲线中的离群点。所以，RANSAC也是一种“外点”检测算法。RANSAC算法是一种不确定算法，它只能在一种概率下产生结果，并且这个概率会随着迭代次数的增加而加大。

算法基本思想和流程: RANSAC是通过反复选择数据集去估计出模型(会计算出很多个模型结果，最后选取数据支持最多的那个作为最优模型），一直迭代到估计出认为比较好的模型。
具体的实现步骤可以分为以下几步：
- 1.选择出可以估计出模型的最小数据集；(对于直线拟合来说就是两个点，对于计算Homography矩阵就是4个点)
- 2.使用这个数据集来计算出数据模型；
- 3.将所有数据带入这个模型，计算出“内点”的数目；(累加在一定误差范围内的适合当前迭代推出模型的数据)
- 4.比较当前模型和之前推出的最好的模型的“内点“的数量，记录最大“内点”数的模型参数和“内点”数；
- 5.重复1-4步，直到迭代结束或者当前模型已经足够好了(“内点数目大于一定数量”)。

迭代次数推导

假设“内点”在数据中的占比为 $t$ :

$t=\frac{n_{\text {inliers }}}{n_{\text {inliers }}+n_{\text {outliers }}}$

确定该问题的模型需要 $n$ 个点，这个 $n$ 是根据问题而定义的，例如拟合直线时 $n$ 为2，平面拟合时 $n$ =3，求解点云之间的刚性变换矩阵时 $n$ =3，求解图像之间的射影变换矩阵是 $n$ =4，等等。 $k$ 表示迭代次数，即随机选取 $n$ 个点计算模型的次数。 $P$ 为在这些参数下得到正确解的概率。为方便表示，我们可以得到， $n$ 个点都是内点的概率为 $t^n$ ，则 $n$ 个点中至少有一个是外点的概率为:
$1-t^{n}$

$1-t^n)^k$ 表示 $k$ 次随机抽样中都没有找到一次全是内点的情况，这个时候得到的是错误解，也就是：

$P=1-\left(1-t^{n}\right)^{k}$

内点概率 $t$ 是一个先验值，可以给出一些鲁棒的值。同时也可以看出，即使 $t$ 给的过于乐观，也可以通过增加迭代次数 $k$ ，来保证正确解的概率 $P$ 。同样的，可以通过上面式子计算出来迭代次数 $k$ ，即假设需要正确概率为 $P$ （例如我们需要99%的概率取到正确解），则：

$k=\frac{\log (1-P)}{\log \left(1-t^{n}\right)}$

  // prev frames cam0 ----------> cam1
  //              |                |
  //              |ransac          |ransac
  //              |   stereo match |
  // curr frames cam0 ----------> cam1
  //
  // 1) Stereo matching between current images of cam0 and cam1.
  // 2) RANSAC between previous and current images of cam0.
  // 3) RANSAC between previous and current images of cam1.

/**
 * @brief 计算原图像帧关键点对应的矫正位置
 * @param pts1：上一时刻的关键点位置
 * @param pts2:当前时刻跟踪匹配到的关键点位置
 * @param R_p_c:根据imu信息计算得到的两个时刻相机的相对旋转信息
 * @param distortion_model,intrinsics：相机内参和畸变模型
 * @param inlier_error：内点可接受的阈值（关键点距离差）
 * @param success_probability：成功的概率
 * @return inlier_markers：内点标志位
 */
void ImageProcessor::twoPointRansac(
    const vector<Point2f>& pts1, const vector<Point2f>& pts2,
    const cv::Matx33f& R_p_c, const cv::Vec4d& intrinsics,
    const std::string& distortion_model,
    const cv::Vec4d& distortion_coeffs,
    const double& inlier_error,
    const double& success_probability,
    vector<int>& inlier_markers)

  // 对cam0和cam1分别进行前后帧2点法ransac
  vector<int> cam0_ransac_inliers(0);
  twoPointRansac(prev_matched_cam0_points, curr_matched_cam0_points,
      cam0_R_p_c, cam0_intrinsics, cam0_distortion_model,
      cam0_distortion_coeffs, processor_config.ransac_threshold,
      0.99, cam0_ransac_inliers);

  vector<int> cam1_ransac_inliers(0);
  twoPointRansac(prev_matched_cam1_points, curr_matched_cam1_points,
      cam1_R_p_c, cam1_intrinsics, cam1_distortion_model,
      cam1_distortion_coeffs, processor_config.ransac_threshold,
      0.99, cam1_ransac_inliers);

请添加图片描述

迭代次数计算参考前文：

$k=\frac{\log (1-P)}{\log \left(1-t^{n}\right)}$

在这里使用的 RANSC 计算模型：

$p_{2}^{T}[t]_{\times} R p_{1}=0$
假设前后帧对应点相机归一化坐标分别为:

$p_{1}=\left[\begin{array}{lll} x 1 & y 1 & 1 \end{array}\right]^{T}, p_{2}=\left[\begin{array}{lll} x 2 & y 2 & 1 \end{array}\right]^{T}$

其中 $R$ = $R_{pre}^{cur}$ 为根据IMU的平均角速度得到的,此时坐标系都统一到一个坐标系下：
$p_{2}^{T}[t]_{\times} R p_{1}= \left[\begin{array}{lll} x_{2} & y_{2} & 1 \end{array}\right] \cdot\left[\begin{array}{ccc} 0 & -t_{z} & t_{y} \\ t_{z} & 0 & -t_{x} \\ -t_{y} & t_{x} & 0 \end{array}\right] \cdot\left[\begin{array}{c} x_{1} \\ y_{1} \\ 1 \end{array}\right]=0$

展开得：

${\color{Green} \begin{bmatrix} y_{1}-y_{2} & -(x_{1}-x_{2}) & x_{1}y_{2}-x_{2}y_{2} \end{bmatrix}}\cdot \begin{bmatrix} t_{x}\\ t_{y}\\ t_{z} \end{bmatrix}=0$

其中绿色部分在代码中对应这一块:

vector<Point2d> pts_diff(pts1_undistorted.size());
  for (int i = 0; i < pts1_undistorted.size(); ++i)
    pts_diff[i] = pts1_undistorted[i] - pts2_undistorted[i];
...  
...
MatrixXd coeff_t(pts_diff.size(), 3);
  for (int i = 0; i < pts_diff.size(); ++i) {
    coeff_t(i, 0) = pts_diff[i].y;
    coeff_t(i, 1) = -pts_diff[i].x;
    coeff_t(i, 2) = pts1_undistorted[i].x*pts2_undistorted[i].y -
      pts1_undistorted[i].y*pts2_undistorted[i].x;
  }

至于这个模型是怎么选出来的呢? 假设一共有N个inliers点对,那么根据对极几何则有 N 个方程，由于误差和outliers的存在,等式右边不可能为零,此时取两个点（两对点）将式子分块,并且只考虑两个点（两对点）的情况,那么将会有:

${\color{Green} \begin{bmatrix} y_{1}-y_{2} & -(x_{1}-x_{2}) & x_{1}y_{2}-x_{2}y_{2}\\ y_{3}-y_{4} & -(x_{3}-x_{4}) & x_{3}y_{4}-x_{4}y_{3} \end{bmatrix}} \cdot \begin{bmatrix} t_{x}\\ t_{y}\\ t_{z} \end{bmatrix}= {\color{Green} \begin{bmatrix} A_{x} \\ A_{y} \\ A_{z} \end{bmatrix}^{T}} \cdot \begin{bmatrix} t_{x}\\ t_{y}\\ t_{z} \end{bmatrix}\approx {\color{Red} \begin{bmatrix} 0\\ 0 \end{bmatrix}}$

那我们可以分别得到以下三个式子:

$\begin{aligned} &{\left[\begin{array}{l} A_{x} \\ A_{y} \end{array}\right]^{T} \cdot\left[\begin{array}{c} t_{x} \\ t_{y} \end{array}\right] \approx- A_{z} \cdot t_{z}} \\ &{\left[\begin{array}{c} A_{x} \\ A_{z} \end{array}\right]^{T} \cdot\left[\begin{array}{c} t_{x} \\ t_{z} \end{array}\right] \approx- A_{y} \cdot t_{y}} \\ &{\left[\begin{array}{c} A_{y} \\ A_{z} \end{array}\right]^{T} \cdot\left[\begin{array}{c} t_{y} \\ t_{z} \end{array}\right] \approx- A_{x} \cdot t_{x}} \end{aligned}$

目标使得误差最小，比较上式中 $A_*$ 部分的大小,取最小的并令模型的平移为1,即令 $t_* = 1$ 。进而直接对系数求逆则是模型的解。之后要做的步骤跟常规RANSAC就十分接近了,找出适应当前模型的所有inliers,然后计算误差并不断迭代找到最好的模型.

    // Construct the model; 构建模型
    Vector2d coeff_tx(coeff_t(pair_idx1, 0), coeff_t(pair_idx2, 0));
    Vector2d coeff_ty(coeff_t(pair_idx1, 1), coeff_t(pair_idx2, 1));
    Vector2d coeff_tz(coeff_t(pair_idx1, 2), coeff_t(pair_idx2, 2));
    vector<double> coeff_l1_norm(3);
    coeff_l1_norm[0] = coeff_tx.lpNorm<1>();
    coeff_l1_norm[1] = coeff_ty.lpNorm<1>();
    coeff_l1_norm[2] = coeff_tz.lpNorm<1>();
    // 从三个式子中，选择系数最小的那个进行计算
    int base_indicator = min_element(coeff_l1_norm.begin(),
        coeff_l1_norm.end())-coeff_l1_norm.begin();

    Vector3d model(0.0, 0.0, 0.0);
    if (base_indicator == 0) {
      Matrix2d A;
      A << coeff_ty, coeff_tz;
      Vector2d solution = A.inverse() * (-coeff_tx);
      model(0) = 1.0;
      model(1) = solution(0);
      model(2) = solution(1);
    } else if (base_indicator ==1) {
      Matrix2d A;
      A << coeff_tx, coeff_tz;
      Vector2d solution = A.inverse() * (-coeff_ty);
      model(0) = solution(0);
      model(1) = 1.0;
      model(2) = solution(1);
    } else {
      Matrix2d A;
      A << coeff_tx, coeff_ty;
      Vector2d solution = A.inverse() * (-coeff_tz);
      model(0) = solution(0);
      model(1) = solution(1);
      model(2) = 1.0;
    }