画像特徴量から縮尺・回転角を推定する
カメラの被写体が元画像に対してどんな角度・縮尺で写っているのか
知りたいことがあります。
が、OpenCV には剛体の回転・拡大/縮小を行う関数がありませんので
新たに作成してみました。
- 手順
1. ORB, SIFT といった画像特徴量を抽出すると、ノルムが最小に近い
点のペアの集合を見つけることができます。
ペアの集合から任意の2組を選び、アフィン変換行列および逆行列
を求めます。(剛体の場合、回転、平行移動、縮尺の4つのパラメータ
で変換が規定されるので、必要な方程式は4つとなる。)
具体的には
を解くことになります。
解法: 2つの点の差分を、
の差分をとするとを
消去できます。2つの式に書き下すと
これからについて解くと
は2つのうちの一方の点のペアから算出できます。もう1つのペアを
検算に使うことができます。
これからアフィン変換行列
を作成します。逆変換は invertAffineTransformation() で計算することができます。
2. 手順1で見つかった全てのペアに対して、変換後の値の誤差を調べます。
誤差が一定値以下のペアの割合が例えば80%以上であればそのアフィン変換を採用します。
そうでなければ別の2組のペアを選択し、同じ手順を繰り返します。
- プログラム
ORB インスタンスを作成し、detector, extractor, matcher を構成します。
matcher.match() を呼び出し vector
この中からノルムが最小に近い(例えば3倍以下)ペアのvectorを算出します。
(ここまでは他の方のブログに記述があるので割愛します。)
#include <opencv2/opencv2.hpp> #include <algorithm> #include <vector> using namespace std; using namespace cv; // 2つのキーポイントの最小距離を求める static double getMinimumDistance(vector<KeyPoint> &kp, vector<DMatch> &m, vector <int> &indexes, int k) { double dmin = DBL_MAX; Point2f pk = kp[m[k].queryIdx].pt; for (int i = 0; i < (int)indexes.size(); ++i) { Point2f pt = kp[m[indexes[i]].queryIdx].pt; double d = fabs(pt.x - pk.x) + fabs(pt.y - pk.y); if (d < dmin) dmin = d; } return dmin; } // 二乗和の計算 static inline float dist2(Point2f &p) { return p.x * p.x + p.y * p.y; } // 剛体のアフィン行列の算出 static bool getRigidTransform(Point2f *xs, Point2f *xd, Mat &mat, float &theta, float &mu) { Point2f ds(xs[0].x - xs[1].x, xs[0].y - xs[1].y); Point2f dd(xd[0].x - xd[1].x, xd[0].y - xd[1].y); if (dist2(ds) < 1e-10 || dist2(dd) < 1e-10) { return false; } mu = sqrt(dist2(dd)/dist2(ds)); float ct, st; float y = ds.y * dd.y + ds.x * dd.x; float x = -ds.x * dd.y + ds.y * dd.x; if (y < 1e-8) { ct = 0.0f; st = 1.0f; theta = M_PI / 2; if (y > 0.0f) { if (x < 0.0f) { theta = -theta; } } else { if (x > 0.0f) { theta = -theta; } } } else { float tt = x / y; theta = atan(tt); st = sqrt(tt * tt / (1.0f + tt * tt)); ct = sqrt(1.0f / (1.0f + tt * tt)); if (x < 0.0f) st = -st; if (y < 0.0f) st = -st; } mat = (Mat_<double>(2, 3) << (mu * ct), (mu * st), (xd[0].x - mu * ( ct * xs[0].x + st * xs[0].y)), (-mu * st), (mu * ct), (xd[0].y - mu * (-st * xs[0].x + ct * xs[0].y))); return true; } // Affine 変換を推定する bool deferAffine(vector<DMatch> &matches_good, vector<KeyPoint> &kpA, vec tor<KeyPoint> &kpB, Mat &af, Mat &invaf, float &mu, float &theta) { int count = 0; vector<int> indexs; // matches_good が少ない時はエラー if (matches_good.size() < 2) { return false; } // 乱数発生 vector<int> seeds; for (int i = 0; i < (int)matches_good.size(); ++i) { seeds.push_back(i); } random_shuffle(seeds.begin(), seeds.end()); vector<int>::iterator ps = seeds.begin(); do { // index を追加する indexs.clear(); int k = 0; for ( ; indexs.size() < 2; ++ps) { if (ps == seeds.end()) { random_shuffle(seeds.begin(), seeds.end()); ps = seeds.begin(); ++count; } int j = *ps; if (indexs.size() == 0) { indexs.push_back(j); } else { double d = getMinimumDistance(kpA, matches_good, indexs, j); if (d > orb_neighborThreshold) { indexs.push_back(j); } } } // 2点以下だったら失敗 if (indexs.size() < 2) { break; } // 仮に affine 変換を求める Point2f pntSrc[2], pntDst[2]; for (int i = 0; i < 2; ++i) { pntSrc[i] = kpA[matches_good[indexs[i]].queryIdx].pt; pntDst[i] = kpB[matches_good[indexs[i]].trainIdx].pt; } mu = 0.0f; theta = 0.0f; if (!getRigidTransform(pntSrc, pntDst, af, theta, mu)) { if (count < 3) { continue; } break; } Mat invaf2mat; invertAffineTransform(af, invaf2mat); int voteCount[2] = { 0, 0 }; for (int i = 0; i < (int)matches_good.size(); ++i) { Point2f pntS = kpA[matches_good[i].queryIdx].pt; Point2f pntD = kpB[matches_good[i].trainIdx].pt; Mat m = (Mat_<double>(3, 1) << pntD.x, pntD.y, 1.0); Mat mr = invaf2mat * m; double dist = fabs(mr.at<double>(0, 0) - pntS.x) + fabs(mr.at<double>(1, 0) - pntS.y); voteCount[ (dist < orb_voteDist) ? 0 : 1 ]++; } double vc = 1.0 * (double)voteCount[0] / (double)(voteCount[0] + voteCount[1]); if (vc > orb_voteWinner) { // affine 変換を返す invaf = invaf2mat; return true; } } while(count < orb_repeatCount); return false; }