3D ビジョン (5): エピポーラ幾何学と三角形分割

3D ビジョン (5): エピポーラ幾何学と三角形分割

エピポーラ ジオメトリは、2 つのビュー間の内部射影関係を記述します。これは外部シーンとは関係がなく、カメラの内部パラメータと 2 つのビュー間の相対的な姿勢にのみ依存します。

1. エピポーラ幾何学

2 枚の画像から一対の点のペアが得られたとします。そのような一致する点のペアが複数あれば、これらの 2 次元画像点の対応関係を通じて 2 つのフレーム間のカメラの動きを復元できます。

ここでの幾何学的関係を代数的な観点から分析します。最初のフレームの座標系では、P の空間位置を次のようにします。
P = [X, Y, Z]

ピンホールカメラモデルによれば、２つの画素点ｐ１、ｐ２の画素位置座標が得られる。ここで、K はカメラの内部参照行列、R と t は 2 つの座標系間のカメラの動きです。
s1p1 = KP
s2p2 = K(RP + t)

s1p1 と p1 は射影関係にあり、同次座標という意味で等しいので、この等しい関係をスケールまで等しいと呼び、s1p1～p1 と表します。したがって、上記 2 つの射影関係は次のように書くことができます:
p1 ～ KP
p2 ～ K(RP + t)

ピクセル座標系の座標を正規化平面座標系の座標に変換します。方程式の左辺と右辺に同時に inv(K) を掛け、x1 = inv(K) p1、x2 = inv(K) p2 を記録します。ここで、x1 と x2 は正規化された 2 つの平面上の座標です。ピクセルを取得し、上式に代入して取得:
x2 ～ R x1 + t

************************************************* *********************************************** ****** *********** 補足: ベクトル a = [a1, a2, a3].T, b = [b1, b2, b3].T と外積を覚えておいてください。 axb は: axb = [a2b3- a3b2 , a3b1 - a1b3, a1b2 - a2b1] = [0, -a3, a2; a3, 0, -a1; -a2, a1, 0] b = a^ b。 ^ 記号を導入してベクトル a を非対称行列 a ^ に変換することで、外積 axb の計算処理を行列とベクトルの乗算 a ^ b に変換し、線形演算に変換します。

---

方程式の両辺に t ^ を同時に乗算することは、両辺の t との外積を同時に行うことと同じであり、方程式の右側の 2 番目の項を削除できます: t ^ x2 ~ t
^ R×1

次に、両辺に同時に x2.T を左に掛けますが、 t ^ x2 は t と x2 の両方に垂直なベクトルなので、x2 と内積すると 0 になります。ゼロ化後、スケールの意味での等号は厳密な等号に変換され、単純な恒等式を取得できます:
x2.T t ^ R x1 = 0

p1 と p2 を再度代入すると、次のようになります。
p2.T inv(K).T t ^ R inv(K) p1 = 0

これら 2 つの公式はエピポーラ制約と呼ばれ、その単純さで有名です。その幾何学的意味は、3 つの点 O1、P、および O2 が同一平面上にあり、逆極の幾何学的拘束には平行移動と回転が同時に含まれるということです。中間部分を 2 つの行列、基本行列 F (基本行列) と必須行列 E (本質行列) として記録するため、極座標制約をさらに単純化できます。

E = t ^ R
F = inv(K).TE inv(K)
x2.TE x1 = 0
p2.TF p1 = 0

エピポーラ制約は 2 つのマッチング点の空間的位置関係を簡潔に与えるため、カメラの姿勢推定問題は次の 2 つのステップに変換されます。 ステップ 1: ペアになった点のピクセル位置に従って、線形方程式を解いて
E またはF .
ステップ 2: 回転行列と行列解析理論の固有の特性に基づいて、E または F から R と t を分解します。

2. 三角測量

三角測量とは、同じランドマークを異なる位置から観察し、観察した位置からランドマークまでの距離を推定することを指します。三角測量はガウスによって最初に提案され、測量に応用されましたが、天文学や地理学にも応用されています。たとえば、天文学では、さまざまな季節の星の角度を観察することで、私たちからの距離を推定できます。コンピューター ビジョンでは、三角測量を使用して、さまざまな写真内のランドマークの位置を観察することで、ランドマークの距離を推定できます。

エピポーラ幾何学の定義に従って、x1 と x2 を 2 つの特徴点の正規化された座標とし、次を満たすとします。
s2 x2 = s1 R x1 + t

エピポーラ幾何学を使用して R と t を解きました。次に、2 つの特徴点の深さ情報 s1 と s2 をさらに取得したいと思います。上の式の左辺に x2 ^ を掛けると、次のようになります。
s2 x2 ^ x2 = 0 = s1 x2 ^ R x1 + x2 ^ t

この式の左辺はゼロであり、右辺は s2 の方程式とみなすことができ、それに従って s2 を直接求めることができます。s2 が解決されると、s1 も非常に簡単に取得でき、2 つのフレームの下の点の深度を取得できます。つまり、それらの 3D 空間座標を決定できます。

3. 実験プロセス

双眼カメラを使用して左目と右目の画像を取得します。

ORB 特徴を抽出し、特徴点をペアにします。

エピポーラ幾何学を使用すると、2 つの画像のポーズ変換関係が次のように取得されます。

三角測量を使用して、最初の画像のカメラ視点を 3D 座標系として取得し、各ランドマーク ポイントの 3D 座標を復元します。

私たちは写真に対応するマーカーポイントの位置にいますが、図は奥行き情報を示しており、カメラに近いほど緑色、カメラから遠いほど赤色となり、中央がグラデーションカラーで表示されます。視覚化効果は次のとおりです。

実験結果によると、三角測量は確かにカメラからランドマークの深さを復元することができますが、計算結果は比較的大まかであり、大まかな距離の尺度しか得られず、メートル単位で正確に記述することは困難であり、距離を正確に記述することは困難です。誤差が大きいです。また、極幾何学アルゴリズムは堅牢ではなく、一度特徴点の組み合わせを間違えると、復元された姿勢変換行列に大きな誤差が生じ、三角測量によって復元された奥行き情報にも大きな誤差が生じ、まったく使用できなくなります。 。

4. ソースコード

エピポーラ幾何学:

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>


using namespace std;
using namespace cv;


// 利用匹配好的特征点对、landmark位置，基于对极几何方法，估计旋转矩阵R、平移向量t

void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,
                          std::vector<KeyPoint> keypoints_2,
                          std::vector<DMatch> matches,
                          const Mat &K, Mat &R, Mat &t) {
    
    

  // 把匹配点对转换为vector<Point2f>的形式
  vector<Point2f> points1;
  vector<Point2f> points2;

  // 将对应landmark标志点的像素坐标，分别push到points1、points2容器中
  
  cout << "匹配点对的像素位置: " << endl;
  for (int i = 0; i < (int) matches.size(); i++) {
    
    
      
    points1.push_back(keypoints_1[matches[i].queryIdx].pt);
    points2.push_back(keypoints_2[matches[i].trainIdx].pt);
    
    cout << keypoints_1[matches[i].queryIdx].pt << "  " << keypoints_2[matches[i].trainIdx].pt << endl;
  }

  // 计算基础矩阵 F
  Mat fundamental_matrix;
  fundamental_matrix = findFundamentalMat(points1, points2);
  cout << endl << "基础矩阵 F: " << endl << fundamental_matrix << endl << endl;

  // 计算本质矩阵 E
  Point2d principal_point(K.at<double>(0, 2), K.at<double>(1, 2));  //相机光心
  double focal_length = K.at<double>(0, 0);      //相机焦距
  
  Mat essential_matrix;
  essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
  cout << "本质矩阵 E: " << endl << essential_matrix << endl << endl;

  // 计算单应矩阵 H，但在本例中场景中，特征点并不都落在同一个平面上，单应矩阵意义不大
  Mat homography_matrix;
  homography_matrix = findHomography(points1, points2, RANSAC, 3);
  cout << "单应矩阵 H: " << endl << homography_matrix << endl << endl;

  // 从本质矩阵E中恢复旋转和平移信息，此函数仅在Opencv3中提供
  recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
  cout << "恢复旋转矩阵 R: " << endl << R << endl << endl;
  cout << "恢复平移向量 t: " << endl << t << endl << endl;

}

三角測量:

#include <iostream>
#include <opencv2/opencv.hpp>


using namespace std;
using namespace cv;


// 保留指定区间的深度信息，此范围内深度估计值较为精确，深度估计值小于low_th的会被赋值成low_th，深度估计值大于up_th的会被赋值成up_th
// 借助rgb颜色可视化像素点位置的深度，绿色表示距离近，红色表示距离远

cv::Scalar get_color(float depth) {
    
    
    
  float up_th = 21, low_th = 16, th_range = up_th - low_th;
  
  if (depth > up_th) depth = up_th;
  if (depth < low_th) depth = low_th;
  
  // 如果距离较近，会接近绿色；如果距离较远，会接近红色
  return cv::Scalar(0, 255 * (1 - (depth - low_th) / th_range), 255 * (depth - low_th) / th_range);
  
}


// 将像素坐标系下的坐标，转换到归一化坐标系下的坐标，(u, v) - (x, y, 1)
// x = (u - cx) / fx
// y = (v - cy) / fy

Point2f pixel2cam(const Point2d &p, const Mat &K) {
    
    
  return Point2f
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}


// 三角测量，计算对极几何尺度s1、s2，还原每个landmark标志点的深度信息，landmark 3d坐标信息存储于points容器中

void triangulation(const vector<KeyPoint> &keypoint_1, 
                   const vector<KeyPoint> &keypoint_2,
                   const std::vector<DMatch> &matches,
                   const Mat &K, const Mat &R, const Mat &t,
                   vector<Point3d> &points) {
    
    
  
  // 默认以第一幅图像的相机坐标系为基准，还原出来的深度信息，就是相对于第一幅图像视角下的深度
    
  Mat T1 = (Mat_<float>(3, 4) <<
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0);
  Mat T2 = (Mat_<float>(3, 4) <<
    R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),
    R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),
    R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0));

  vector<Point2f> pts_1, pts_2;
  
  // 将像素坐标转换成归一化平面坐标，存储于pts_1、pts_2
  for (DMatch m:matches) {
    
    
    pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));
    pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));
  }
  
  // cv::triangulatePoints函数，输出的3D坐标是齐次坐标，共四个维度，因此需要将前三个维度除以第四个维度，得到非齐次坐标x、y、z
  Mat pts_4d;
  cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);

  // 转换成非齐次坐标
  for (int i = 0; i < pts_4d.cols; i++) {
    
    
      
    Mat x = pts_4d.col(i);
    x /= x.at<float>(3, 0); // 归一化，四个分量全除以最后一位，将第四位数值转化为1
    
    Point3d p(x.at<float>(0, 0), x.at<float>(1, 0), x.at<float>(2, 0));
    points.push_back(p);
    
  }
}

5. プロジェクトリンク

コードが機能しない場合、または私が作成したデータセットを直接使用したい場合は、プロジェクト リンクをダウンロードできます:
https://blog.csdn.net/Twilight737