Mugichoko's blog

Mugichoko’s blog

プログラミングを中心としたメモ書き.

Random Sample Consensus (RANSAC) のお勉強

私が学生の頃に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]を利用した.尚,先にも書いた通り,この実装はあくまでも理解を深めるためのものであって実用的ではないことに注意されたい.実用的にはOpenCVcv::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日)