当前位置:   article > 正文

教你一点点掌握视觉三维重建-colmap 重要代码逐行解析(大纲-预热阶段)_colmap 代码 中文注释

colmap 代码 中文注释

教你一点点掌握视觉三维重建-colmap 重要代码逐行解析

这里以colmap 框架为准,主要对其重要环节代码结合自己的想法进行逐一讲解,colmap 作为目前state-of-the-art 的视觉重建pipeline,本人将其代码分为两个大环节:前端和后端.前端主要是特征提取和匹配,后端包括三角化,Register,BA等环节.


  • 完成时间deadline:12月底;这里以博客为证,如果您感觉到这份博客帮助到您,希望给予鼓励,我也会利用周末及其下班时间尽快完成整个文章的撰写
    -欢迎关注B站:橙子的RGB空间,B 站后续也会同步每一章节的视频讲解,同时B站有很多成果视频可以提供大家观看!!!
    同时文档会同步github: 地址,欢迎star!!!!!!!!!!!!!!

目录

  1. 特征提取

  2. 特征匹配

  3. 三角化或前方交会

  4. 运动恢复

  5. 光束法平差及算法改进


前端

feature extracting

  • sift-GPU 算法

      待写
    
    • 1
  • DSP-SIFT算法

       待写
    
    • 1

    Domain-size pooling SIFT 是从多个尺度的SIFT 描述子进行一个平均,
    参考论文:Domain-Size Pooling in Local Descriptors and Network Architectures,
    其已被证明优于其他SIFT算法的变种及一些深度学习算子.

feature matching

  • 匹配方法

       待写
    
    • 1
  • 几何验证-剔除outliers

    1 . 对于标定相机,利用E/F 、H/F 、H/E的内点个数比值来决定使用哪种模型来剔除误点

    代码最终 inlier_matches = ExtractInlierMatches(matches,模型内点数,模型mask)

      //  对于标定相机 利用 E/F H/F H/E 的内点比值来决定使用那种模型来剔除outliers
    
      const double E_F_inlier_ratio =
          static_cast<double>(E_report.support.num_inliers) /
          F_report.support.num_inliers;
      const double H_F_inlier_ratio =
          static_cast<double>(H_report.support.num_inliers) /
          F_report.support.num_inliers;
      const double H_E_inlier_ratio =
          static_cast<double>(H_report.support.num_inliers) /
          E_report.support.num_inliers;
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11

    2 . 对于非标相机,利用F 矩阵模型

  // 非标相机估计 H/F 的比值即可,E 矩阵无法得到
  const double H_F_inlier_ratio =
      static_cast<double>(H_report.support.num_inliers) /
      F_report.support.num_inliers;

  if (H_F_inlier_ratio > options.max_H_inlier_ratio) {
    config = ConfigurationType::PLANAR_OR_PANORAMIC;
  } else {
    config = ConfigurationType::UNCALIBRATED;
  }

  inlier_matches = ExtractInlierMatches(matches, F_report.support.num_inliers,
                                        F_report.inlier_mask);
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13

对于外点的剔除,除了利用ransac 估计模型来剔除outliers ,
还有可改进的思路吗?众所周知,ransac 是每次进行随机抽样,重新计算模型,这样就比较耗时.

后端

trianglulation

colmap 代码中,三角化分为以下几个函数:

1 . 两帧三角化 ,即使用SVD 分解求解P(M*P=m),代码如下:

// proj_matrix1 ,proj_matrix2 分别是世界系到图像系的投影矩阵
Eigen::Vector3d TriangulatePoint(const Eigen::Matrix3x4d& proj_matrix1,
                                 const Eigen::Matrix3x4d& proj_matrix2,
                                 const Eigen::Vector2d& point1,
                                 const Eigen::Vector2d& point2) {
  Eigen::Matrix4d A;

  A.row(0) = point1(0) * proj_matrix1.row(2) - proj_matrix1.row(0);
  A.row(1) = point1(1) * proj_matrix1.row(2) - proj_matrix1.row(1);
  A.row(2) = point2(0) * proj_matrix2.row(2) - proj_matrix2.row(0);
  A.row(3) = point2(1) * proj_matrix2.row(2) - proj_matrix2.row(1);

  Eigen::JacobiSVD<Eigen::Matrix4d> svd(A, Eigen::ComputeFullV);

  return svd.matrixV().col(3).hnormalized();
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16

2 . 多帧三角化 ,即是求解超定方程组.从多个观测值恢复3D 场景结构,三角化需要满足以下两个条件:

(1) 所有观测值需要满足 cheirality constraint (什么是cheirality constraint ,即是正景深约束,
多视图几何书中分解E 矩阵的时候,会得到四种结果-R,t,但是重构点在相机前方的结果只有一个)

(2) 观测对之间有足够的三角化角度

对于求解的结果需要满足以上两个条件,代码如下

条件1 : xyz cheirality constraint

    for (const auto& pose : pose_data) {
      if (!HasPointPositiveDepth(pose.proj_matrix, xyz)) {
        return std::vector<M_t>();
      }
    }
  • 1
  • 2
  • 3
  • 4
  • 5

条件2: 像对之间需足够的三角化角度(阈值为min_tri_angle)

    for (size_t i = 0; i < pose_data.size(); ++i) {
      for (size_t j = 0; j < i; ++j) {
        const double tri_angle = CalculateTriangulationAngle(
            pose_data[i].proj_center, pose_data[j].proj_center, xyz);
        if (tri_angle >= min_tri_angle_) {
          return std::vector<M_t>{xyz};
        }
      }
    }
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9

3 . 鲁棒性LO-RANSAC 多帧三角化,其主要是剔除一些错误的匹配点,
这个也是colmap 框架三角化一个特色,代码如下:

bool EstimateTriangulation(
    const EstimateTriangulationOptions& options,
    const std::vector<TriangulationEstimator::PointData>& point_data,
    const std::vector<TriangulationEstimator::PoseData>& pose_data,
    std::vector<char>* inlier_mask, Eigen::Vector3d* xyz) {
  CHECK_NOTNULL(inlier_mask);
  CHECK_NOTNULL(xyz);
  CHECK_GE(point_data.size(), 2);
  CHECK_EQ(point_data.size(), pose_data.size());
  options.Check();

  // Robustly estimate track using LORANSAC.
  LORANSAC<TriangulationEstimator, TriangulationEstimator,
           InlierSupportMeasurer, CombinationSampler>
      ransac(options.ransac_options);
  ransac.estimator.SetMinTriAngle(options.min_tri_angle);
  ransac.estimator.SetResidualType(options.residual_type);
  ransac.local_estimator.SetMinTriAngle(options.min_tri_angle);
  ransac.local_estimator.SetResidualType(options.residual_type);
  const auto report = ransac.Estimate(point_data, pose_data);
  if (!report.success) {
    return false;
  }

  *inlier_mask = report.inlier_mask;
  *xyz = report.model;

  return report.success;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29

Register or motion recover

  • 基本矩阵FundamentalMatrix

    基本矩阵的求解从以下公式出发:

x ′ T F x = 0 \mathbf{x}^{\prime T} F \mathbf{x}=0 xTFx=0

以上公式可以写成包含9个未知数的线性齐次方程,如下:

u i T f = 0 u i = [ u i u i ′ , v i u i ′ , u i ′ , u i v i ′ , v i v i ′ , v i ′ , u i , v i , 1 ] T f = [ F 11 , F 12 , F 13 , F 21 , F 22 , F 23 , F 31 , F 32 , F 33 ] T

\begin{aligned} &\mathbf{u}_{i}^{T} \mathbf{f}=0\\ &\begin{aligned} \mathbf{u}_{i} &=\left[u_{i} u_{i}^{\prime}, v_{i} u_{i}^{\prime}, u_{i}^{\prime}, u_{i} v_{i}^{\prime}, v_{i} v_{i}^{\prime}, v_{i}^{\prime}, u_{i}, v_{i}, 1\right]^{T} \\ \mathbf{f} &=\left[F_{11}, F_{12}, F_{13}, F_{21}, F_{22}, F_{23}, F_{31}, F_{32}, F_{33}\right]^{T} \end{aligned}
\end{aligned} uiTf=0uif=[uiui,viui,ui,uivi,vivi,vi,ui,vi,1]T=[F11,F12,F13,F21,F22,F23,F31,F32,F33]T

读过MVG的人都知道基本矩阵由于一个尺度不确定和其秩等于2,故其DOF 等于7.所以估计基本矩阵的算法根据不忽略rank-2 constraint 和忽略rank-2 constraint,求解方法可分为7点法和8点法或者more points 法

  1. 不忽略rank-2 constraint 的7点法,其核心是确定lambda 和mu,其中lambda + mu =1;由上式可以看出,9个未知数,7个方程,明显是无法求解,那么由于rank=2 的限制,那么F 矩阵的null space 可以由f1和f2的线性组合表示,故有下式(参考论文-Determining the Epipolar Geometry and its Uncertainty: A Review, International Journal of Computer Vision):

F = α F 1 + ( 1 − α ) F 2 \mathbf{F}=\alpha \mathbf{F}_{1}+(1-\alpha) \mathbf{F}_{2} F=αF1+(1α)F2

det ⁡ [ α F 1 + ( 1 − α ) F 2 ] = 0 \operatorname{det}\left[\alpha \mathbf{F}_{1}+(1-\alpha) \mathbf{F}_{2}\right]=0 det[αF1+(1α)F2]=0

代码如下:

std::vector<FundamentalMatrixSevenPointEstimator::M_t>
FundamentalMatrixSevenPointEstimator::Estimate(
    const std::vector<X_t>& points1, const std::vector<Y_t>& points2) {
  CHECK_EQ(points1.size(), 7);
  CHECK_EQ(points2.size(), 7);

 
  Eigen::Matrix<double, 7, 9> A;
  for (size_t i = 0; i < 7; ++i) {
    const double x0 = points1[i](0);
    const double y0 = points1[i](1);
    const double x1 = points2[i](0);
    const double y1 = points2[i](1);
    A(i, 0) = x1 * x0;
    A(i, 1) = x1 * y0;
    A(i, 2) = x1;
    A(i, 3) = y1 * x0;
    A(i, 4) = y1 * y0;
    A(i, 5) = y1;
    A(i, 6) = x0;
    A(i, 7) = y0;
    A(i, 8) = 1;
  }

  // 9 个未知数,7个方程, 所以我们有两个null space .
  Eigen::JacobiSVD<Eigen::Matrix<double, 7, 9>> svd(A, Eigen::ComputeFullV);
  const Eigen::Matrix<double, 9, 9> f = svd.matrixV();
  Eigen::Matrix<double, 1, 9> f1 = f.col(7);
  Eigen::Matrix<double, 1, 9> f2 = f.col(8);

  f1 -= f2;
 // 很明显,方程个数不够,无法求解,所以我们必须增加约束
 // det(F) = det(lambda * f1 + (1 - lambda) * f2)
 // 其中lambda + mu = 1

  const double t0 = f1(4) * f1(8) - f1(5) * f1(7);
  const double t1 = f1(3) * f1(8) - f1(5) * f1(6);
  const double t2 = f1(3) * f1(7) - f1(4) * f1(6);
  const double t3 = f2(4) * f2(8) - f2(5) * f2(7);
  const double t4 = f2(3) * f2(8) - f2(5) * f2(6);
  const double t5 = f2(3) * f2(7) - f2(4) * f2(6);

  Eigen::Vector4d coeffs;
  coeffs(0) = f1(0) * t0 - f1(1) * t1 + f1(2) * t2;
  coeffs(1) = f2(0) * t0 - f2(1) * t1 + f2(2) * t2 -
              f2(3) * (f1(1) * f1(8) - f1(2) * f1(7)) +
              f2(4) * (f1(0) * f1(8) - f1(2) * f1(6)) -
              f2(5) * (f1(0) * f1(7) - f1(1) * f1(6)) +
              f2(6) * (f1(1) * f1(5) - f1(2) * f1(4)) -
              f2(7) * (f1(0) * f1(5) - f1(2) * f1(3)) +
              f2(8) * (f1(0) * f1(4) - f1(1) * f1(3));
  coeffs(2) = f1(0) * t3 - f1(1) * t4 + f1(2) * t5 -
              f1(3) * (f2(1) * f2(8) - f2(2) * f2(7)) +
              f1(4) * (f2(0) * f2(8) - f2(2) * f2(6)) -
              f1(5) * (f2(0) * f2(7) - f2(1) * f2(6)) +
              f1(6) * (f2(1) * f2(5) - f2(2) * f2(4)) -
              f1(7) * (f2(0) * f2(5) - f2(2) * f2(3)) +
              f1(8) * (f2(0) * f2(4) - f2(1) * f2(3));
  coeffs(3) = f2(0) * t3 - f2(1) * t4 + f2(2) * t5;

  Eigen::VectorXd roots_real;
  Eigen::VectorXd roots_imag;
  if (!FindPolynomialRootsCompanionMatrix(coeffs, &roots_real, &roots_imag)) {
    return {};
  }

  std::vector<M_t> models;
  models.reserve(roots_real.size());

  for (Eigen::VectorXd::Index i = 0; i < roots_real.size(); ++i) {
    const double kMaxRootImag = 1e-10;
    if (std::abs(roots_imag(i)) > kMaxRootImag) {
      continue;
    }

    const double lambda = roots_real(i);
    const double mu = 1;

    Eigen::MatrixXd F = lambda * f1 + mu * f2;

    F.resize(3, 3);

    const double kEps = 1e-10;
    if (std::abs(F(2, 2)) < kEps) {
      continue;
    }

    F /= F(2, 2);

    models.push_back(F.transpose());
  }

  return models;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  1. 忽略rank-2 constraint 就成了8点法或者更多点法,这个时候我们就可以使用最小二乘来求解,即是

min ⁡ F ∑ i ( m ~ i ′ T F m ~ i ) 2 \min _{\mathbf{F}} \sum_{i}\left(\tilde{\mathbf{m}}_{i}^{\prime T} \mathbf{F} \tilde{\mathbf{m}}_{i}\right)^{2} minFi(m~iTFm~i)2

min ⁡ f ∥ U n f ∥ 2 \min _{\mathrm{f}}\left\|\mathbf{U}_{n} \mathbf{f}\right\|^{2} minfUnf2

现在f只 受一个未知尺度的影响,为了避免f=0,我们必须增加约束,即使SVD 中V 的最后一个特征值等于0(详见<<计算机视觉中的多视图几何>>书本281页),代码如下:

std::vector<FundamentalMatrixEightPointEstimator::M_t>
FundamentalMatrixEightPointEstimator::Estimate(
    const std::vector<X_t>& points1, const std::vector<Y_t>& points2) {
  CHECK_EQ(points1.size(), points2.size());

  // 中心归一化是为了数值稳定性
  std::vector<X_t> normed_points1;
  std::vector<Y_t> normed_points2;
  Eigen::Matrix3d points1_norm_matrix;
  Eigen::Matrix3d points2_norm_matrix;
  CenterAndNormalizeImagePoints(points1, &normed_points1, &points1_norm_matrix);
  CenterAndNormalizeImagePoints(points2, &normed_points2, &points2_norm_matrix);

  //解决线性方程 x2' * F * x1 = 0.
  Eigen::Matrix<double, Eigen::Dynamic, 9> cmatrix(points1.size(), 9);
  for (size_t i = 0; i < points1.size(); ++i) {
    cmatrix.block<1, 3>(i, 0) = normed_points1[i].homogeneous();
    cmatrix.block<1, 3>(i, 0) *= normed_points2[i].x();
    cmatrix.block<1, 3>(i, 3) = normed_points1[i].homogeneous();
    cmatrix.block<1, 3>(i, 3) *= normed_points2[i].y();
    cmatrix.block<1, 3>(i, 6) = normed_points1[i].homogeneous();
  }

  // SVD 分解
  Eigen::JacobiSVD<Eigen::Matrix<double, Eigen::Dynamic, 9>> cmatrix_svd(
      cmatrix, Eigen::ComputeFullV);
  const Eigen::VectorXd cmatrix_nullspace = cmatrix_svd.matrixV().col(8);
  const Eigen::Map<const Eigen::Matrix3d> ematrix_t(cmatrix_nullspace.data());

  // 增加约束,即SVD 中的V 的特征向量最后一个等于0 
  Eigen::JacobiSVD<Eigen::Matrix3d> fmatrix_svd(
      ematrix_t.transpose(), Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Vector3d singular_values = fmatrix_svd.singularValues();
  singular_values(2) = 0.0;
  const Eigen::Matrix3d F = fmatrix_svd.matrixU() *
                            singular_values.asDiagonal() *
                            fmatrix_svd.matrixV().transpose();

  const std::vector<M_t> models = {points2_norm_matrix.transpose() * F *
                                   points1_norm_matrix};
  return models;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 本质矩阵

    ( 1) 5点法求解

          待写
    
    • 1

    ( 2) 8点法求解

          待写
    
    • 1
  • 单应矩阵

    单应矩阵的求解相对于本质和基本就相对简单,在colmap 代码中直接利用DLT算法直接求解,理论公式如下:
    ( u v 1 ) = H ( x y 1 ) \left(

    uv1
    \right)=H\left(
    xy1
    \right) uv1=Hxy1

其中H:

H = [ h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 9 ] H=\left[

h1h2h3h4h5h6h7h8h9
\right] H=h1h4h7h2h5h8h3h6h9

线性化展开得到:
− h 1 x − h 2 y − h 3 + ( h 7 x + h 8 y + h 9 ) u = 0 -h_{1} x-h_{2} y-h_{3}+\left(h_{7} x+h_{8} y+h_{9}\right) u=0 h1xh2yh3+(h7x+h8y+h9)u=0
− h 4 x − h 5 y − h 6 + ( h 7 x + h 8 y + h 9 ) v = 0 -h_{4} x-h_{5} y-h_{6}+\left(h_{7} x+h_{8} y+h_{9}\right) v=0 h4xh5yh6+(h7x+h8y+h9)v=0

整理可得:
A i = ( − x − y − 1 0 0 0 u x u y u 0 0 0 − x − y − 1 v x v y v ) A_{i}=\left(

xy1000uxuyu000xy1vxvyv
\right) Ai=(x0y0100x0y01uxvxuyvyuv)
h = ( h 1 h 2 h t 3 h 4 h 5 h 6 h 7 h 8 h 9 ) h=\left(
h1h2ht3h4h5h6h7h8h9
\right)_{}
h=(h1h2ht3h4h5h6h7h8h9)

利用最小二乘求解超定方程组便可求,代码如下:

std::vector<HomographyMatrixEstimator::M_t> HomographyMatrixEstimator::Estimate(
    const std::vector<X_t>& points1, const std::vector<Y_t>& points2) {
  CHECK_EQ(points1.size(), points2.size());

  const size_t N = points1.size();

  // 中心归一化,数值稳定性
  std::vector<X_t> normed_points1;
  std::vector<Y_t> normed_points2;
  Eigen::Matrix3d points1_norm_matrix;
  Eigen::Matrix3d points2_norm_matrix;
  CenterAndNormalizeImagePoints(points1, &normed_points1, &points1_norm_matrix);
  CenterAndNormalizeImagePoints(points2, &normed_points2, &points2_norm_matrix);

  //构建方程
  Eigen::Matrix<double, Eigen::Dynamic, 9> A = Eigen::MatrixXd::Zero(2 * N, 9);

  for (size_t i = 0, j = N; i < points1.size(); ++i, ++j) {
    const double s_0 = normed_points1[i](0);
    const double s_1 = normed_points1[i](1);
    const double d_0 = normed_points2[i](0);
    const double d_1 = normed_points2[i](1);

    A(i, 0) = -s_0;
    A(i, 1) = -s_1;
    A(i, 2) = -1;
    A(i, 6) = s_0 * d_0;
    A(i, 7) = s_1 * d_0;
    A(i, 8) = d_0;

    A(j, 3) = -s_0;
    A(j, 4) = -s_1;
    A(j, 5) = -1;
    A(j, 6) = s_0 * d_1;
    A(j, 7) = s_1 * d_1;
    A(j, 8) = d_1;
  }

  // SVD 求解
  Eigen::JacobiSVD<Eigen::Matrix<double, Eigen::Dynamic, 9>> svd(
      A, Eigen::ComputeFullV);

  const Eigen::VectorXd nullspace = svd.matrixV().col(8);
  Eigen::Map<const Eigen::Matrix3d> H_t(nullspace.data()); // 单应矩阵

  const std::vector<M_t> models = {points2_norm_matrix.inverse() *
                                   H_t.transpose() * points1_norm_matrix};
  return models;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • Estimate Relative Pose (2D-2D)

     待写
    
    • 1
  • Estimate Absolute Pose(2D-3D)

     待写
    
    • 1

Bundle adjustment

  • 经典的重投影误差

         待写
    
    • 1
  • pose 为常量的重投影误差

         待写
    
    • 1
  • 部分参数优化(如R or T)的重投影误差~自己改进

        待写
    
    • 1
  • GPS 约束的重投影误差(SfM 融合gps)~自己改进

         待写
    
    • 1
  • GCP 约束的重投影误差( Marker SfM)~自己改进

       待写
    
    • 1
  • IMU 角度约束的重投影误差~自己改进

        待写
    
    • 1
声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/喵喵爱编程/article/detail/768595
推荐阅读
相关标签
  

闽ICP备14008679号