[データ関連付け] パッチに基づく対応する特徴の関連付け、現在のフレーム -> 参照フレームの関連付け、フレーム間トラッキング

ここに画像の説明を挿入

フレーム間トラッキングとデータ関連付け

1. WarpPixelWise (現在のフレームの特徴点の位置を見つける)

1.1 機能

これは、現在のフレームのカメラポーズに従って、参照フレームの特徴点の周囲のピクセルを現在のフレームに投影し、そのピクセル値を指定された配列に格納します。

1.2 関数の入出力

入力: 関数の入力には、現在のフレーム、参照フレーム、参照フレーム内の特徴点、参照フレームと現在のフレームのピラミッドレベル、ハーフパッチサイズが含まれます出力: 配列へのポインター出力
ピクセル値の。

1.3 アルゴリズムのステップ

参照フレーム内の特徴点から参照フレームカメラまでの距離と、現在のフレームカメラまでの距離を計算します。
参照フレームのカメラ座標系の特徴点の座標を 3 次元空間に逆投影し、カメラまでの距離に応じてスケーリングして 3 次元座標を取得します。
3 次元座標を現在のフレームのカメラ座標系に変換し、それを現在のフレーム画像に投影してピクセル座標を取得します。
ピクセル座標を検索レベルにスケールし、パッチサイズの半分以内のピクセルごとに次の操作を実行します。
- ピクセル座標を参照フレームカメラ座標系の 3 次元座標に変換します。
- 3 次元座標を参照フレーム画像に投影してピクセル座標を取得します。
- ピクセル座標を参照フレームのピラミッドレベルにスケールします。
- 双一次補間に従って参照フレームイメージ上のこのピクセルの値を計算し、出力配列に格納します。

数式は次のように説明されます。

$\mathbf{p}_{cur} = \mathbf{T}_{cur}^{world} \mathbf{T}_{ref}^{world^{-1}} \mathbf{p}_{ref}$

$\mathbf{p}_{ref} = \frac{d_{ref}}{\|\mathbf{p}_{ref}^{cam}\|}\mathbf{p}_ {参照}^{カム}$

$\mathbf{p}_{cur}^{search} = \frac{\mathbf{p}_{cur}}{2^{level_{cur}}}$

$\mathbf{p}_{ele}^{search} = \mathbf{p}_{ele}^{patch} + \mathbf{p}_{cur}^{search}$

$\mathbf{p}_{ele}^{cam} = \frac{\mathbf{p}_{ele}^{search}}{2^{level_{cur}}}$

$\mathbf{p}_{ele}^{world} = \mathbf{T}_{ref}^{world} \mathbf{T}_{cur}^{world^{ -1}} \mathbf{p}_{ele}^{cam}$

$\mathbf{p}_{ele}^{ref} = \frac{1}{2^{level_{ref}}}\mathbf{K}_{ref}\mathbf{p} _{エレ}^{カム}$

$I(\mathbf{p}_{ele}^{ref}) = \\ w_ {00}I(\lfloor \mathbf{p}_{ele,x}\rfloor,\lfloor \mathbf{p}_{ele,y}\rfloor) + w_{01}I(\lfloor \mathbf{p }_{ele,x}\rfloor,\lfloor \mathbf{p}_{ele,y}\rfloor+1) + \\w_{10}I(\lfloor \mathbf{p}_{ele,x} \rfloor+1,\lfloor \mathbf{p}_{ele,y}\rfloor) + w_{11}I(\lfloor \mathbf{p}_{ele,x}\rfloor+1,\lfloor \mathbf {p}_{ele,y}\rfloor+1)$

ここで、 $\mathbf{T}$ カメラのポーズ、 $\mathbf{p}$ ピクセル座標を表し、 $d は$ 距離を表します。 $\|\cdot\|$ $\mathbf{K}$ の係数を表します $K は$ カメラの内部参照行列 $I(\cdot)$ は、画像上の特定のピクセルの値、 $w_{00},w_{01},w_{10},w_{11} を$ 双一次補間の重みを表します。

bool WarpPixelWise(const Frame& cur_frame, const Frame& ref_frame, const FeatureWrapper& ref_ftr,
    const int level_ref, const int level_cur, const int half_patch_size, uint8_t* patch) {
    
    
  double depth_ref = (ref_frame.pos() - ref_ftr.landmark->pos()).norm();
  double depth_cur = (cur_frame.pos() - ref_ftr.landmark->pos()).norm();

  // back project to 3D points in reference frame
  Eigen::Vector3d xyz_ref;
  ref_frame.cam()->backProject3(ref_ftr.px, &xyz_ref);
  xyz_ref = xyz_ref.normalized() * depth_ref;

  // project to current frame and convert to search level
  Eigen::Vector3d xyz_cur = cur_frame.T_cam_world() * (ref_frame.T_cam_world().inverse()) * xyz_ref;
  Eigen::Vector2d px_cur;
  cur_frame.cam()->project3(xyz_cur, &px_cur);
  Eigen::Vector2d px_cur_search = px_cur / (1 << level_cur);

  // for each pixel in the patch(on search level):
  // - convert to image level
  // - back project to 3D points
  // - project to ref frame and find pixel value in ref level
  uint8_t* patch_ptr = patch;
  const cv::Mat& img_ref = ref_frame.img_pyr_[level_ref];
  const int stride = img_ref.step.p[0];

  for (int y = -half_patch_size; y < half_patch_size; ++y) {
    
    
    for (int x = -half_patch_size; x < half_patch_size; ++x, ++patch_ptr) {
    
    
      const Eigen::Vector2d ele_patch(x, y);
      Eigen::Vector2d ele_search = ele_patch + px_cur_search;
      Eigen::Vector3d ele_xyz_cur;
      cur_frame.cam()->backProject3(ele_search * (1 << level_cur), &ele_xyz_cur);
      ele_xyz_cur = ele_xyz_cur.normalized() * depth_cur;
      Eigen::Vector3d ele_xyz_ref =
          ref_frame.T_cam_world() * (cur_frame.T_cam_world().inverse()) * ele_xyz_cur;
      Eigen::Vector2d ele_ref;
      ref_frame.cam()->project3(ele_xyz_ref, &ele_ref);
      ele_ref = ele_ref / (1 << level_ref);

      const int xi = std::floor(ele_ref[0]);
      const int yi = std::floor(ele_ref[1]);
      if (xi < 0 || yi < 0 || xi + 1 >= img_ref.cols || yi + 1 >= img_ref.rows) {
    
    
        VLOG(200) << "ref image: col-" << img_ref.cols << ", row-" << img_ref.rows;
        VLOG(200) << "xi: " << xi << ", "
                  << "yi: " << yi;
        return false;
      } else {
    
    
        const float subpix_x = ele_ref[0] - xi;
        const float subpix_y = ele_ref[1] - yi;
        const float w00 = (1.0f - subpix_x) * (1.0f - subpix_y);
        const float w01 = (1.0f - subpix_x) * subpix_y;
        const float w10 = subpix_x * (1.0f - subpix_y);
        const float w11 = 1.0f - w00 - w01 - w10;
        const uint8_t* const ptr = img_ref.data + yi * stride + xi;
        *patch_ptr = static_cast<uint8_t>(
            w00 * ptr[0] + w01 * ptr[stride] + w10 * ptr[1] + w11 * ptr[stride + 1]);
      }
    }
  }

  return true;
}

2. GetWarpMatrixAffine (現在のフレーム -> 参照フレームのアフィン変換行列を計算)

2.1 機能

この関数の役割は、現在のフレームの参照フレームの特徴点の投影が現在のフレームの対応するパッチと一致するように、現在のフレームから参照フレームへのアフィン変換行列を計算することです。

2.2 関数の入出力

入力: 関数の入力には、参照フレームと現在のフレームのカメラパラメーター、参照フレーム内の特徴点、特徴点の方向ベクトル、特徴点から参照フレームのカメラまでの距離、姿勢が含まれます。現在のフレームと参照フレームの間の変換、および参照フレームのピラミッドレベル。
Output: 出力アフィン変換行列へのポインタ。

2.3 アルゴリズムのステップ

関数の実装手順は次のとおりです。

特徴点の方向ベクトルと距離に従って 3 次元座標を計算します。
参照フレーム内で水平および垂直方向に沿ってパッチサイズの半分を移動し、移動したピクセル座標を 3 次元空間に逆投影して、2 方向の 3 次元ベクトルを取得します。
3 次元ベクトルを現在のフレームのカメラ座標系に変換し、それを現在のフレームに投影して 3 つのピクセル座標を取得します。
3 つのピクセル座標からアフィン変換行列を計算します。

数式は次のように説明されます。

$\begin{aligned} \mathbf{x}_{ref} &= \mathbf{f}_{ref} d_{ref} \\ \ mathbf{x}_{du,ref} &= \mathbf{J}_{du,ref} \mathbf{x}_{ref} \\ \mathbf{x}_{dv,ref} &= \mathbf{ J}_{dv,ref} \mathbf{x}_{ref} \\ \mathbf{x}_{cur} &= T_{cur\_ref} \mathbf{x}_{ref} \\ \mathbf{ x}_{du,cur} &= T_{cur\_ref} \mathbf{x}_{du,ref} \\ \mathbf{x}_{dv,cur} &= T_{cur\_ref} \mathbf {x}_{dv,ref} \\ \mathbf{A}_{cur\_ref} &= \begin{bmatrix}\frac{\mathbf{x}_{du,cur}-\mathbf{x}_{cur}}{kHalfPatchSize} & \frac{\mathbf{x}_{dv,cur}-\mathbf{x}_{cur}}{kHalfPatchSize}\end{bmatrix} \end{整列}$

ここで $\mathbf{f}_{ref}$ 特徴点の方向ベクトルを表します、 $d_{ref}$ 特徴点から参照フレームカメラまでの距離を示します $\mathbf{x}_{ref}$ $\mathbf{x}_{du,ref}$ における特徴点の 3 次元座標を示します。 $バツ_{あなた、参照し}$ 和 $\mathbf{x}_{dv,ref}$ 水平方向と垂直方向にパッチサイズの半分を移動した後の参照フレーム内の 3 次元座標を示します。 $\mathbf{J}_{du,ref}$ 和 $\mathbf{J}_{dv,ref}$ パッチサイズの半分を水平方向と垂直方向に移動した後の参照フレームカメラ座標系の対応するピクセル座標に対応する 3 次元ベクトルをそれぞれ表します、 T cur_ref T_{cur\_ref $T_{キュル_リフレ__}$ 現在のフレームと参照フレーム間のポーズ変換を表します、 $\mathbf{x}_{cur}$ 、 $\mathbf{x}_{du,cur}$ 和 $cur\mathbf{x}_{dv,cur}$ それぞれ $\mathbf{x}_{ref}を表します$ 、 $\mathbf{x}_{du,ref}$ 和 $\mathbf{x}_{dv,ref}$ 現在のフレームのカメラ座標系に変換して得られる 3 次元座標 $\frac{\mathbf{x}_{du,cur}-\mathbf{x}_ {cur}} {kHalfPatchSize}$ 和 $\frac{\mathbf{x}_{dv,cur}-\mathbf{x}_{cur}}{kHalfPatchSize}$ はそれぞれ、水平方向と垂直方向にパッチサイズの半分を移動した後の、現在のフレームのカメラ座標系の対応するピクセル座標に対応する 3 次元ベクトルを表します。 A cur _ ref $\mathbf{A}_{cur\_ref}$ 現在のフレームから参照フレームへのアフィン変換行列を表します。

void GetWarpMatrixAffine(const CameraPtr& cam_ref, const CameraPtr& cam_cur,
    const Eigen::Ref<Keypoint>& px_ref, const Eigen::Ref<BearingVector>& f_ref,
    const double depth_ref, const Transformation& T_cur_ref, const int level_ref,
    AffineTransform* A_cur_ref) {
    
    
  CHECK_NOTNULL(A_cur_ref);

  // Compute affine warp matrix A_ref_cur
  const int kHalfPatchSize = 5;
  const Position xyz_ref = f_ref * depth_ref;
  Position xyz_du_ref, xyz_dv_ref;
  // NOTE: project3 has no guarantee that the returned vector is unit length
  // - for pinhole: z component is 1 (unit plane)
  // - for omnicam: norm is 1 (unit sphere)
  cam_ref->backProject3(
      px_ref + Eigen::Vector2d(kHalfPatchSize, 0) * (1 << level_ref), &xyz_du_ref);
  cam_ref->backProject3(
      px_ref + Eigen::Vector2d(0, kHalfPatchSize) * (1 << level_ref), &xyz_dv_ref);
  if (cam_ref->getType() == Camera::Type::kPinhole) {
    
    
    xyz_du_ref *= xyz_ref[2];
    xyz_dv_ref *= xyz_ref[2];
  } else {
    
    
    xyz_du_ref.normalize();
    xyz_dv_ref.normalize();
    xyz_du_ref *= depth_ref;
    xyz_dv_ref *= depth_ref;
  }

  Keypoint px_cur, px_du_cur, px_dv_cur;
  cam_cur->project3(T_cur_ref * xyz_ref, &px_cur);
  cam_cur->project3(T_cur_ref * xyz_du_ref, &px_du_cur);
  cam_cur->project3(T_cur_ref * xyz_dv_ref, &px_dv_cur);
  A_cur_ref->col(0) = (px_du_cur - px_cur) / kHalfPatchSize;
  A_cur_ref->col(1) = (px_dv_cur - px_cur) / kHalfPatchSize;
}

3. GetWarpMatrixAffine (現在のフレーム -> 参照フレームのアフィン変換行列を計算)

3.1 機能

この関数の機能は、最高のパフォーマンスを得るためにどのピラミッドレベルで一致点を検索するかを計算することです。

3.2 関数の入出力

入力: その入力には、アフィン変換行列と最大ピラミッドレベルが含まれます。
出力: 出力はピラミッドレベルです。

3.3 アルゴリズムのステップ

アフィン変換行列の行列式 D を計算します。
D が 3.0 より大きく、検索レベルが最大ピラミッドレベルより小さい場合は、D が 3.0 以下になるか、検索レベルが最大ピラミッドレベルに達するまで、検索レベルに 1 を加えて D に 0.25 を乗算します。

数式は次のように説明されます。

$search\_level = \min(\max\_level, \max\{n|D) (A_{cur\_ref}) > 3.0^n\})$

ただし、 $D(A_{cur\_ref})$ アフィン変換行列 $A_{cur\_ref}$ の決定要因。

この関数の原理は、アフィン変換行列の不確実性に応じて、検索するピラミッドレベルを選択することです。アフィン変換行列がより不確実な場合、より高いマッチングパフォーマンスを得るためにより高いピラミッドレベルで検索する必要があります。アフィン変換行列がより明確である場合、計算効率を向上させるために下位のピラミッドレベルで検索を実行できます。


int GetBestSearchLevel(const AffineTransform& A_cur_ref, const int max_level) {
    
    
  // Compute patch level in other image
  int search_level = 0;
  double D = A_cur_ref.determinant();
  while (D > 3.0 && search_level < max_level) {
    
    
    search_level += 1;
    D *= 0.25;
  }
  return search_level;
}