私が学生の頃にRANSACに関して頭の整理のためにまとめた資料です．実装も含んでいますが，あくまでも理解を深めるためです．OpenCVの実装を使う方が信頼性や実行速度の面で有利ですので悪しからず．
基本事項
RANdom SAmple Consensus (RANSAC) は，1981年にM. A. FischlerとR. C. Bollesによって提案された[1]ロバスト推定法の1つである．RANSACを利用することで，簡単には「最小二乗法を利用する際に悪影響を与える外れ値（モデルに当てはまらないデータ）を排除すること」ができる．
最小二乗法に有効なデータはインライア (Inlier)，外れ値はアウトライア (Outlier) と呼ばれる．また，ここで言うモデルとは，例えば (Perspective-n-Point) PnP問題を解くとか，ホモグラフィを推定するとか，自分が当てはめたい問題のことを指す．
アルゴリズム
個人的には，Wikipedia[2]の例が一番分かりやすい．下記は，その和訳である．
入力:
data – 観測データ
model – 観測データに適合するモデル
n – モデルに適合するのに必要な最小のデータ数
k – アルゴリズムでの繰り返し回数
t – データが適合したと決定するための閾値
d – データが適合したと断定するのに必要なデータの数
出力:
best_model – データに最も適合するモデルパラメータ（良いモデルが見つからなければnil）
best_consensus_set – このモデルが推定してきたデータ
best_error – データに対するこのモデルで得られるエラー
iterations := 0
best_model := nil
best_consensus_set := nil
best_error := infinity
while iterations < k
maybe_inliers := dataからランダムに選ばれたn個のデータ群
maybe_model := maybe_inliersを適用したモデルパラメータ
consensus_set := maybe_inliers
for maybe_inliersを含まない全データに対して以下を行う
if 閾値tより小さいエラーでモデルに当てはまるデータである
consensus_setにそのデータを加える
if consensus_setの要素数 > d
（これは，暗に，良いモデルが見つかったとして，これからそれがどれくらい良いものかを試すことに当たる）
this_model := consensus_set中の全データを用いて得られるモデルパラメータ
this_error := this_modelが上のデータに対してどれくらい適合するかの量
if this_error < best_error
（今までの中で最も良いモデルを得た，より良いものが見つかるまでこれを繰り返す）
best_model := this_model
best_consensus_set := consensus_set
best_error := this_error
iterationsをインクリメントする
return best_model, best_consensus_set, best_error
PnP問題の例
アルゴリズム
モデルにPnP問題を当てはめた場合，アルゴリズムは以下の様になる．
入力:
all_correspondences – 特徴点の3次元位置と2次元位置（対応点）の集合
M – PnPで得られる位置姿勢
n – PnPを解くのに必要な特徴点数（P4Pなら4，P6Pなら6）
k – アルゴリズムでの繰り返し回数
t – 対応点がはずれ値でないと決定するための閾値（再投影誤差等）
d – 対応点がはずれ値でないと断定するのに必要なデータの数
出力:
best_M – 最終的な位置姿勢（良い位置姿勢が得られなければnil）
best_consensus_set – 上記の位置姿勢において再投影誤差が少ない対応点
best_reprojection_error – 上記の対応点での再投影誤差（の平均等）
iterations := 0
best_M := nil
best_consensus_set := nil
best_reprojection_error := infinity
while iterations < k
maybe_inliers := dataからランダムに選ばれたn個の対応点
maybe_M := maybe_inliersでPnPを解いた時の位置姿勢
consensus_set := maybe_inliers
for maybe_inliersを含まない全対応点に対して以下を行う
if 対応点の上記の位置姿勢における再投影誤差が閾値tより小さいconsensus_setにその対応点を加える
if consensus_setの要素数 > d
（これは，暗に，良い位置姿勢が得られたとして，これからそれがどれくらい良いものかを試すことに当たる）
this_M := consensus_set中の全対応点を用いて得られる位置姿勢
this_reprojection_error := this_Mでの再投影誤差（の平均等）
if this_reprojection_error < best_reprojection_error
（今までの中で最も良い位置姿勢を得た，より良いものが見つかるまでこれを繰り返す）
best_M := this_M
best_consensus_set := consensus_set
best_reprojection_error := this_reprojection_error
iterationsをインクリメントする
return best_M, best_consensus_set, best_reprojection_error
実装例 (C++)
この例ではPnPを解くためにEPnP[3]を利用した．尚，先にも書いた通り，この実装はあくまでも理解を深めるためのものであって実用的ではないことに注意されたい．実用的にはOpenCVのcv::solvePnPRansac()を用いるべし．
RANSAC.h
// // RANSAC (RANdom SAmple Consensus) // This program is an simple implementation of RANSAC algorithm // [1] http://en.wikipedia.org/wiki/RANSAC // [2] http://www.ai.sri.com/pubs/files/836.pdf // // >> How to use // const INTRINSICS myIntrinsics = {uc, vc, fu, fv}; // CRANSAC myRANSAC(w, z, myIntrinsics); // double R[3][3], t[3], err; vector<int> cs_indeces; // myRANSAC.get_consensus_set(vec3D, vec2D, cs_indeces, R, t, err); // // 3/4/2012 // Mugichoko // #ifndef _RANSAC #define _RANSAC #include <iostream> #include <cstdlib> // srand() #include <ctime> #include "epnp.h" // http://cvlab.epfl.ch/software/EPnP/ #include "Hash.h" #define M 6 // # of points used for solving PnP #define THRESHOLD 1.0 // Error tolerance: Reprojection error in pixel, which RANSAC allows to have using namespace std; typedef struct { double uc, vc, fu, fv; // In pixel } INTRINSICS; class CRANSAC { private: double m_w; // (number of inliers in data) / (number of points in data) double m_z; // Assurance: at least one of our random selections is an error-free set of n data points INTRINSICS m_intrinsics; epnp m_EPnP; // FUNCTIONS double dot(const double * v1, const double * v2); // Refer to epnp::dot() double reprojection_error( const cv::Point3d & pt3D, // Input: X, Y, and Z const cv::Point2d & pt2D, // Input: u and v const double (&R)[3][3], const double (&t)[3]); // Input: Rotation matrix and translation vector public: // CONSTRUCTOR & DESTRUCTOR CRANSAC(const double w, const double z, const INTRINSICS & intrinsics); ~CRANSAC(); // FUNCTIONS void set_probabilities(const double w, const double z); void set_intrinsics(const INTRINSICS & intrinsics); void get_consensus_set( const vector<cv::Point3d> & vec3D, // Input: 3D positions of correspondences const vector<cv::Point2d> & vec2D, // Input: 2D positions of correspondences vector<int> & cs_indeces, // Output: Indeces of consensus set vector<bool> & cs_bool, // Output: If a point is included in consensus set, then true double (&R)[3][3], // Output: Rotation matrix of scene model, NOT camera!! double (&t)[3], // Output: Translation vector of scene model, NOT camera!! double & reprojection_error); // Output: Reprojection error }; #endif
RANSAC.cpp
#include "RANSAC.h" ////////////////////////////// // CONSTRUCTOR & DESTRUCTOR // ////////////////////////////// CRANSAC::CRANSAC(const double w, const double z, const INTRINSICS & intrinsics) { // GENERATE RANDOM VALUES srand((unsigned)time(NULL)); // Initialize this->set_probabilities(w, z); this->set_intrinsics(intrinsics); } CRANSAC::~CRANSAC() { } /////////////// // FUNCTIONS // /////////////// void CRANSAC::set_probabilities(const double w, const double z) { this->m_w = w; this->m_z = z; } void CRANSAC::set_intrinsics(const INTRINSICS & intrinsics) { this->m_intrinsics = intrinsics; this->m_EPnP.set_internal_parameters(intrinsics.uc, intrinsics.vc, intrinsics.fu, intrinsics.fv); } void CRANSAC::get_consensus_set(const vector<cv::Point3d> & vec3D, // Input: 3D positions of correspondences const vector<cv::Point2d> & vec2D, // Input: 2D positions of correspondences vector<int> & cs_indeces, // Output: Indeces of consensus set vector<bool> & cs_bool, // Output: If a point is included in consensus set, then true double (&R)[3][3], // Output: Rotation matrix double (&t)[3], // Output: Translation vector double & reprojection_error) // Output: Reprojection error { reprojection_error = 1000.0; ////////////////////////////////// // CHECK SIZE OF SETS OF POINTS // ////////////////////////////////// if(vec3D.size() != vec2D.size() || !(vec3D.size() >= M && vec2D.size() >= M)) { cerr << "[RANSAC] ERROR!! INVALID ARGUMENT!!" << endl; return; } ////////////////////////// // CALCULATE THRESHOLDS // ////////////////////////// int k = (int)(log(1.0 - this->m_z) / log(1.0 - pow(this->m_w, M)) + 0.5); // Max # of loop int d = (int)(vec3D.size() * this->m_w); // Refer to p.389 of RANSAC paper #ifdef _DEBUG cout << "[RANSAC] NUM OF LOOP: " << k << ", NUM OF NEEDED PTS: " << d << endl; #endif ///////////////////////////// // LOOP STARTS FROM HERE!! // ///////////////////////////// for(int cnt = 0; cnt < k; cnt++) { CHash hash_table_of_maybe_inliers((int)vec3D.size()); // Make a hash table //////////////////////////////////////////////////// // RANDOMLY SELECT M CORRESPONDENCES TO SOLVE PNP // //////////////////////////////////////////////////// // * IT IS ASSUMED THAT CORRESPONDENCES SELECTED // * RANDOMLY HERE ARE ELEMENTS OF CONSENSUS SET vector<int> this_cs_indeces; // Indeces of consensus set are accumulated in this this->m_EPnP.set_maximum_number_of_correspondences(M); this->m_EPnP.reset_correspondences(); for(int i = 0; i < M; i++) { int idx = rand() % (int)vec3D.size(); // * IF THIS HAS BEEN ALREADY SELECTED BEFORE, THEN SELECT THE OTHER ONE if(hash_table_of_maybe_inliers.search(idx)) { i--; } // * IF NOT, USE THIS else { // * THIS IS MAYBE AN INLIER this->m_EPnP.add_correspondence(vec3D[idx].x, vec3D[idx].y, vec3D[idx].z, vec2D[idx].x, vec2D[idx].y); // * REGISTER THEM IN HASH TABLE hash_table_of_maybe_inliers.add(idx); this_cs_indeces.push_back(idx); } } // * SOLVE PNP USING THE M CORRESPONDENCES double maybe_R[3][3], maybe_t[3]; this->m_EPnP.compute_pose(maybe_R, maybe_t); ////////////////////////////////////////////////////////////////////////// // SELECT CORRESPONDENCES EXCEPT THE M CORRESPONDENCES FROM INITIAL SET // ////////////////////////////////////////////////////////////////////////// for(int idx = 0; idx < vec3D.size() ; idx++) { // * NEVER USE THE M CORRESPONDENCES if(!hash_table_of_maybe_inliers.search(idx)) { // * IF REPROJECTION ERROR IS SMALLER THAN THRESHOLD... if(this->reprojection_error(vec3D[idx], vec2D[idx], maybe_R, maybe_t) < THRESHOLD) { // * THEN REGISTER hash_table_of_maybe_inliers.add(idx); this_cs_indeces.push_back(idx); } } } #ifdef _DEBUG cout << "[RANSAC] # of inliers: " << this_cs_indeces.size() << endl; #endif ////////////////////////////////////////////////////////////////////////////////////////// // IF # OF ELEMENTS IN CONSENSUS SET IS LARGER THAN THRESHOLD, EVALUATE THE ORIENTATION // ////////////////////////////////////////////////////////////////////////////////////////// if(this_cs_indeces.size() >= d) { // * SOLVE PNP USING ALL CONSENSUS SET this->m_EPnP.set_maximum_number_of_correspondences((int)this_cs_indeces.size()); this->m_EPnP.reset_correspondences(); vector<int>::iterator itr_this_consensus_set = this_cs_indeces.begin(); while(itr_this_consensus_set != this_cs_indeces.end()) { if(hash_table_of_maybe_inliers.search(*itr_this_consensus_set)) { this->m_EPnP.add_correspondence(vec3D[*itr_this_consensus_set].x, vec3D[*itr_this_consensus_set].y, vec3D[*itr_this_consensus_set].z, vec2D[*itr_this_consensus_set].x, vec2D[*itr_this_consensus_set].y); } itr_this_consensus_set++; } double this_R[3][3], this_t[3]; double this_reprojection_error = this->m_EPnP.compute_pose(this_R, this_t); // * IF CALCULATED ORIENTATION IS BETTER THAN THAT EVER OBTAINED, THEN UPDATE if(this_reprojection_error < reprojection_error) { #ifdef _DEBUG cout << "[RANSAC] CONSENSUS SET FOUND!!" << endl; #endif for(int i = 0; i < 3; i++) { for(int j = 0; j < 3; j++) { R[i][j] = this_R[i][j]; } t[i] = this_t[i]; } reprojection_error = this_reprojection_error; cs_indeces = this_cs_indeces; // STORE BOOLS cs_bool.clear(); for(int cnt = 0; cnt < vec3D.size(); cnt++) { if(hash_table_of_maybe_inliers.search(cnt)) { cs_bool.push_back(true); } else { cs_bool.push_back(false); } } } } } // END OF FOR() } double CRANSAC::dot(const double * v1, const double * v2) { return (v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]); } double CRANSAC::reprojection_error(const cv::Point3d & pt3D, const cv::Point2d & pt2D, const double (&R)[3][3], const double (&t)[3]) { double XYZw[3] = {pt3D.x, pt3D.y, pt3D.z}; // Point's position in world coordinate double Xc = dot(R[0], XYZw) + t[0]; // Point's position in camera coordinate double Yc = dot(R[1], XYZw) + t[1]; // Point's position in camera coordinate double inv_Zc = 1.0 / (dot(R[2], XYZw) + t[2]); // Point's position in camera coordinate double ue = this->m_intrinsics.uc + this->m_intrinsics.fu * Xc * inv_Zc; double ve = this->m_intrinsics.vc + this->m_intrinsics.fv * Yc * inv_Zc; return sqrt((pt2D.x - ue) * (pt2D.x - ue) + (pt2D.y - ve) * (pt2D.y - ve)); }
Hash.h
// // This hash program is based on "open address method", but is not same as // the original one, which has a delete method. This program only checks // whether there is data you want or not in a hash table, or add a data to the table // if there's no data on the table. In addition, this program assumes that there's // enough space of table. // // 1/1/2012 // Mugichoko // #include <iostream> // STATUS #define STATE_EMPTY -1 // Status: Table is empty // //struct bucket_index //{ // int key; // int data; //}; class CHash { private: int m_tableSize; int * m_tableStatus; // INITIALIZATION void initTable() { for(int i = 0; i < this->m_tableSize; i++) { this->m_tableStatus[i] = STATE_EMPTY; } } // HASH // * Generate a new bucket_index. int hash(int key) { return (key % this->m_tableSize); } public: // CONSTRUCTOR AND DESTRUCTOR CHash(){}; CHash(int tableSize) { this->m_tableSize = tableSize; m_tableStatus = new int[tableSize]; this->initTable(); } ~CHash() { delete(this->m_tableStatus); } // SEARCH // * Search a data with key. // * Return true if the data you want is in the table, and // * return false if the data you want is not in the table. bool search(int key) { int bucket_index = this->hash(key); int table_key = this->m_tableStatus[bucket_index]; // THIS IS THE DATA YOU WANT if(table_key == key) { return true; } // IF THIS BUCKET_INDEX IS EMPTY else if(table_key == STATE_EMPTY) { return false; } // IF THERE IS ALREADY THE OTHER DATA IN THE BUCKET_INDEX else { return (this->search(bucket_index + 1)); // Rehash } } // ADD bool add(int key) { int bucket_index = this->hash(key); bool result = this->search(key); // IF THE BUCKET IN THE TABLE IS EMPTY if(!result) { this->m_tableStatus[bucket_index] = key; return true; } else { return false; } } };
参考文献
[1] M. A. Fischler and R. C. Bolles: Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography, Communications of the ACM, Vol. 24, No. 6, pp.381-395, 1981.
[2] RANSAC, Wikipedia（最終確認日：2015年7月1日）
[3] EPnP: Efficient Perspective-n-Point Camera Pose Estimation, Comuter Vision Laboratory CVLAB（最終確認日：2015年7月14日）
