對極幾何的基礎矩陣和本質矩陣之實戰

2021-02-21 CV有道 
 Eigen::Vector3d  pix2cam(Eigen::Matrix3d& K, Eigen::Vector3d& pix){        Eigen::Vector3d cam = K.inverse()*pix;        cam.array() /= cam(2);        return cam;    }
 void find_essential_dlt(std::vector<Eigen::Vector3d>& camPoints1,                            std::vector<Eigen::Vector3d>& camPoints2, Eigen::Matrix3d& E){        assert(camPoints1.size()==camPoints2.size());        using namespace Eigen;
        int N = camPoints1.size();        MatrixXd A(N, 9);
        for (int i = 0; i <N; ++i) {            Vector3d& p1 =camPoints1.at(i);            Vector3d& p2 =camPoints2.at(i);
            A(i, 0) = p1[0]*p2[0];            A(i, 1) = p1[1]*p2[0];            A(i, 2) = p2[0];            A(i, 3) = p1[0]*p2[1];            A(i, 4) = p1[1]*p2[1];            A(i, 5) = p2[1];            A(i, 6) = p1[0];            A(i, 7) = p1[1];            A(i, 8) = 1.0;        }        // SVD 分解        Eigen::JacobiSVD<MatrixXd> solver(A, Eigen::ComputeFullU|Eigen::ComputeFullV);        MatrixXd V = solver.matrixV();        VectorXd X = V.col(8);
        Matrix3d E_;        E_ << X(0), X(1), X(2), X(3), X(4), X(5), X(6), X(7), X(8);        Eigen::JacobiSVD<Matrix3d> solverF(E_, Eigen::ComputeFullU|Eigen::ComputeFullV);        Matrix3d V_ = solverF.matrixV();        Matrix3d U_ = solverF.matrixU();        Vector3d sig_ = solverF.singularValues();        Matrix3d S_=Matrix3d::Zero();        double s = (sig_(0)+sig_(1))/2;        S_(0, 0) = s;        S_(1, 1) = s;        S_(2, 2) = 0;        E = U_ * S_* V_.transpose();    }
根據E矩陣分解相機的姿態：
首先獲得4中可能的結果組合
 void decompose_from_essential(Eigen::Matrix3d& E, Eigen::Matrix3d& R1,Eigen::Matrix3d& R2,  Eigen::Vector3d& t1, Eigen::Vector3d& t2){
        using namespace Eigen;                Matrix3d R_90, R_90_;        R_90 << 0, -1, 0, 1, 0, 0, 0, 0, 1;        R_90_ << 0, 1, 0, -1, 0, 0, 0, 0, 1;
                Eigen::JacobiSVD<Matrix3d> solver(E, Eigen::ComputeFullU|Eigen::ComputeFullV);        Matrix3d U = solver.matrixU();        Matrix3d V = solver.matrixV();        Vector3d sig = solver.singularValues();
        Matrix3d S=Matrix3d::Zero();        S(0, 0) = sig(0);        S(1, 1) = sig(1);        S(2, 2) = sig(2);                if(U.determinant()<0.0){            for(int i=0; i<3; ++i)                U(i, 2) = -U(i, 2);        }        if(V.determinant()<0.0){            for(int i=0; i<3; ++i)                V(i, 2) = -V(i, 2);        }
                R1 = U*R_90*V;        R2 = U*R_90_*V;                t1 = U.col(2);        t2 = -U.col(2);
                double m10 = sqrt(R1.col(0).transpose()*R1.col(0));        double m11 = sqrt(R1.col(1).transpose()*R1.col(1));        double m12 = sqrt(R1.col(2).transpose()*R1.col(2));
        R1.array().col(0)  /= m10;        R1.array().col(1)  /= m11;        R1.array().col(2)  /= m12;
        double m20 = sqrt(R2.col(0).transpose()*R2.col(0));        double m21 = sqrt(R2.col(1).transpose()*R2.col(1));        double m22 = sqrt(R2.col(2).transpose()*R2.col(2));
        R2.array().col(0)  /= m20;        R2.array().col(1)  /= m21;        R2.array().col(2)  /= m22;
        double mt1 = sqrt(t1.transpose()*t1);        t1.array() /= mt1;
        double mt2 = sqrt(t2.transpose()*t2);        t2.array() /= mt2;
    }
其實通過這種方式分解求出的R和t，很難確定他的尺度，所以可以把R和t  進行歸一化處理。 
左相機的外參為[I, 0] ， 右相機有4個可能[R1, t1]  [R2, t1] [R1, t2] [R2, t2].
選擇的思路是可以拿出若干像素點對，首先用三角化求出3維世界坐標點，然後把世界坐標點通過兩個相機的外參，轉換為對應的相機坐標，根據z值的正負進行判別，如果都為正，則符合條件。我這裡選擇了一對點，如果匹配的點比較多，並且有可能有錯誤的匹配，可以通過統計的方式，根據符合條件的點的數量作為衡量依據
   bool choose_pose(Eigen::Matrix3d& K1, Eigen::Matrix3d& R1, Eigen::Vector3d& t1,            Eigen::Matrix3d& K2,  Eigen::Matrix3d& R2, Eigen::Vector3d& t2,            Eigen::Vector3d& p1, Eigen::Vector3d& p2){
                Eigen::Matrix<double, 3, 4> P1, P2;        P1 =  Eigen::Matrix<double, 3, 4>::Zero();        P2 =  Eigen::Matrix<double, 3, 4>::Zero();
        P1.array().block(0, 0, 3, 3) = R1;        P1.array().block(0, 3, 3, 1) = t1;        P1 = K1*P1;
        P2.array().block(0, 0, 3, 3) = R2;        P2.array().block(0, 3, 3, 1) = t2;        P2 = K2*P2;
        Eigen::Vector3d out;                triangulatePoint(P1, P2, p1, p2, out);                        Eigen::Vector3d cam =  R2*out+t2;        
        if(out(2)>0 && cam(2)>0)            return true;        else            return false;
    }
為了測試我用了OpenCV自帶的左右相機的棋盤格圖像中的一幅，內參用的也是雙目標定的內參。
具體使用: fundamental_essential_test.cpp

#include "Stereo/stereo.h"using namespace SLAM_STEREO;using namespace cv;using namespace Eigen;

int main() {      std::string baseRoot="/home/atway/soft/opencv4.1/opencv-4.1.0/samples/data/";    Mat leftImg = imread(baseRoot+"left01.jpg", cv::IMREAD_GRAYSCALE);    Mat rightImg = imread(baseRoot+"right01.jpg", cv::IMREAD_GRAYSCALE);    Size  patternSize(9, 6);        Mat leftCameraMatrix, leftCameraDistCoeffs;    Mat rightCameraMatrix, rightCameraDistCoeffs;
    FileStorage fs(baseRoot+"intrinsics.yml", FileStorage::READ);    if( fs.isOpened() )    {        fs["M1"] >> leftCameraMatrix;        fs["D1"] >> leftCameraDistCoeffs;        fs["M2"] >> rightCameraMatrix;        fs["D2"] >>  rightCameraDistCoeffs;        fs.release();    }    else        std::cout << "Error: can not load the intrinsic parameters\n";
    std::vector<Point2f> leftImgCorners, rightImgCorners;    Mat drawLeftImg, drawRightImg;        SLAM_STEREO::findCorners(leftImg, patternSize, leftImgCorners,drawLeftImg);    SLAM_STEREO::findCorners(rightImg, patternSize, rightImgCorners,drawRightImg);
        std::vector<Point2f> leftUndistImgCorners, rightUndistImgCorners;    cv::undistortPoints(leftImgCorners, leftUndistImgCorners, leftCameraMatrix, leftCameraDistCoeffs);    cv::undistortPoints(rightImgCorners, rightUndistImgCorners, rightCameraMatrix, rightCameraDistCoeffs);
    std::vector<Eigen::Vector3d> leftUndistImgCornersEigen, rightUndistImgCornersEigen;    for (int i = 0; i < leftImgCorners.size(); ++i) {        Point2f& pt1 = leftUndistImgCorners.at(i);        Point2f& pt2 = rightUndistImgCorners.at(i);        leftUndistImgCornersEigen.push_back(Eigen::Vector3d(pt1.x, pt1.y, 1.));        rightUndistImgCornersEigen.push_back(Eigen::Vector3d(pt2.x, pt2.y, 1.));    }
        {                Matrix3d F;        find_fundamental_dlt(leftUndistImgCornersEigen, rightUndistImgCornersEigen, F);        std::cout << "fundamental:" <<  F << std::endl;
                for(int i=0; i<leftImgCorners.size(); ++i){            float d = rightUndistImgCornersEigen[i].transpose()*F*leftUndistImgCornersEigen[i];            std::cout << i << " fundamental epipolar constraint: " << d << std::endl;        }
    }
    std::vector<Vector3d> camPoints1, camPoints2;       {        Eigen::Matrix3d E, K1, K2;        cv::cv2eigen(leftCameraMatrix, K1);        cv::cv2eigen(rightCameraMatrix, K2);

                for(int i=0; i<leftUndistImgCornersEigen.size(); ++i){            camPoints1.push_back(pix2cam(K1, leftUndistImgCornersEigen[i]));            camPoints2.push_back(pix2cam(K2, rightUndistImgCornersEigen[i]));        }                find_essential_dlt(camPoints1, camPoints2, E);        std::cout << "essential:" << E<< std::endl;
                Matrix3d R1, R2;        Vector3d t1, t2;        decompose_from_essential(E, R1, R2, t1, t2);
                Matrix3d I = Matrix3d ::Identity();        Vector3d t0 = Vector3d::Zero();
                {
            Matrix3d R;            Vector3d t;            {                bool ok1 = choose_pose(K1, I, t0, K2, R1, t1, leftUndistImgCornersEigen[0], rightUndistImgCornersEigen[0]);                bool ok2 = choose_pose(K1, I, t0, K2, R1, t2, leftUndistImgCornersEigen[0], rightUndistImgCornersEigen[0]);                bool ok3 = choose_pose(K1, I, t0, K2, R2, t1, leftUndistImgCornersEigen[0], rightUndistImgCornersEigen[0]);                bool ok4 = choose_pose(K1, I, t0, K2, R2, t2, leftUndistImgCornersEigen[0], rightUndistImgCornersEigen[0]);                std::cout <<" ok1 " << ok1<<  " ok2 " << ok2 << " ok3 " << ok3<< " ok4 " << ok4<< std::endl;                if(ok1) {                    R = R1; t=t1;                }else if(ok2){                    R=R1; t=t2;                }else if(ok3){                    R=R2; t=t1;                }else if(ok4){                    R=R2; t=t2;                }            }
                        Matrix3d txR = corss(t)*R;            for(int i=0; i<camPoints1.size(); ++i){                Vector3d& p1 = camPoints1[i];                Vector3d& p2 = camPoints2[i];                float d= p2.transpose()*txR*p1;                std::cout << i << " E epipolar constraint: " << d << std::endl;            }
        }
    }
    return 0;}
時間倉促，代碼難免會有錯誤，如有疑問可以在github上提交。
源碼地址：
https://github.com/zhaitianyong/slam_algorithm/tree/master/Stereo
Happy ～～～

相關焦點

  • 對極幾何的基礎矩陣和本質矩陣2
    上一篇文章介紹了基礎矩陣F和本質矩陣E理論推導，下面主要介紹如何求解Ｆ矩陣。
  • 線性代數基礎-矩陣
    矩陣A稱為2行3列的矩陣.將A矩陣的行列交換獲得的矩陣稱為A矩陣的轉置，如下:就是一個對稱矩陣.     矩陣加法: 第一點矩陣的形狀都要一樣為m*n形的才可相加, 計算方法就是對應元素相加,結果放回原來的位置即可.    矩陣減法同加法, 對應元素相減.    矩陣乘法: 必須是m*n與n*a的形式,即第一個矩陣的列與第二個矩陣的行相同才可以相乘.乘積為m*a形的矩陣. 計算方法如下:
  • ML基礎:協方差矩陣!
    在翻譯sklearn文檔 2.無監督學習部分過程中，發現協方差矩陣幾乎貫穿整個章節，但sklearn指導手冊把協方差部分放在了這一章節偏後的部分，作為機器學習一個基礎概念，在這篇文章中，想把協方差矩陣的相關知識以及主要應用。統計學中常用平均值，方差，標準差等描述數據。
  • 推薦系統 | 矩陣分解(SVD)原理和實戰
    本文收錄在推薦系統專欄，專欄系統化的整理推薦系統相關的算法和框架，並記錄了相關實踐經驗，所有代碼都已整理至推薦算法實戰集合(hub-recsys
  • 矩陣分解 (乘法篇)
    LDU分解在LU的基礎上， 如果我們再進一步，引入對角矩陣（Diagonal）D， 那麼LU分解就變成了LDU分解。 是不是很直觀？這也很容易去解釋， 為什麼LU分解可以寫成對角線全1的三角矩陣， 因為可以提取出一個對角矩陣， 然後乘到左邊下三角矩陣或者右邊上三角矩陣去。 因此， 從本質上來說LDU分解和LU分解沒有差別。
  • 矩陣與矩陣乘積簡介
    這與創建一維數組的不同之處在於所使用的方括號數。與向量一樣，可以訪問Numpy數組的shape屬性：A.shape可以看到該形狀包含兩個數字：它們分別對應於行數和列數。要獲得矩陣元素，需要兩個索引：一個引用行索引，一個引用列索引。使用Numpy，索引過程與向量的索引過程相同，只需要指定兩個索引。
  • 生命之樹卡巴拉是黑暗勢力蓄意刪減的系統(超越矩陣-神聖幾何系列一)
    這是由位於土星上的耶和華集體來監督和管理的，耶和華集體在其權力金字塔的最頂層推動了撒旦的血祭崇拜。本質上，透特和路西法的議程是徹底摧毀所有的有機創造編碼、基數12的矩陣和文物，並將其替換為他們自己的基數10版本。卡巴拉生命樹代表了黑太陽回歸血統基於10D扭曲編碼的基因藍圖，黑太陽回歸血統是許多源自天龍血統的實體，以及來自馬爾戴克和火星戰爭的難民種族。
  • 關聯矩陣、迴路矩陣和割集矩陣的關係
    對於同一個電路，若各支路，節點的編號及方向均相同時，其列寫出的關聯矩陣，迴路矩陣和割集矩陣之間存在著一定的聯繫。對於成對出現在迴路和割集中的支路，如果二條支路方向與迴路一致，（此時對應行中二個元素同號），則該二條支路與割集方向必一正一反（此時
  • 齊次矩陣和齊次坐標
    在機器人及自動駕駛中，經常用齊次變換矩陣將旋轉和平移進行統一。「齊次坐標表示是計算機圖形學的重要手段之一，它既能夠用來明確區分向量和點，同時也更易用於進行仿射（線性）幾何變換。」—— F.S. Hill, JR齊次坐標就是將一個原本n維的向量用一個n+1維的向量來表示，是指一個用於投影幾何裡的坐標系統，如同用於歐氏幾何裡的笛卡兒坐標一般。
  • 光子矩陣的基本概念
    光子並不是構成萬物的本質，只是意識的本質，構成萬物的本質其實還是原子，但是只有原子是無法產生意識的。單純解釋矩陣這個詞的話，你身邊的空氣、水、火焰、桌子、椅子，都是以各種不同的元素，以規律的數學模式模型，組合而成的。這些分子結構就是一種矩陣羅列形式，當然也可以輕易的轉化成數學和化學方程式。
  • 2.4——矩陣的行列式與伴隨矩陣
    在很多教材也先講矩陣，再講矩陣的行列式。此外，由於行列式的每個元素對應唯一的一個代數餘子式，因此，定義了伴隨矩陣，伴隨矩陣與原矩陣及其行列式有密切的關係，為逆矩陣的討論奠定了重要的基礎。知識要點：方陣行列式的性質
  • 天元傑出講堂 | 隨機矩陣與SYK模型
    報告題目：隨機矩陣與SYK模型報告人：田剛 院士報告時間：10月23日下午1：30報告地點：數學樓二樓報告廳
  • 機器學習 線性代數基礎 | 1.4 矩陣乘向量的新視角:變換基底
    我們即將步入這一章的尾聲，在本章前面的三個小節中，我們學習了矩陣和向量的表示方法以及加法、乘法等基本運算的規則，並能夠熟練利用Python語言工具正確的對其進行描述和表示。但是顯然我們不能僅僅滿足於此，那麼在這一小節裡，我們一起回過頭來靜靜的思考這樣一個問題：矩陣A和列向量x的乘法運算Ax有沒有其他更深層次的幾何含義呢？
  • 拋棄複雜的運算來理解「矩陣乘法」和「行列式」
    這是《機器學習中的數學基礎》系列的第4篇。上一篇我們知道了，矩陣其實就是一個線性變換，矩陣的各列就是由原來的基向量變換而來。在此基礎上，假如我們把兩個矩陣放在一起，也就是說，讓兩個矩陣做乘法，它又有什麼含義呢？矩陣乘法我們還是先從矩陣和向量的乘法看起：矩陣乘以一個向量，得到一個新向量。如果我們把新向量再乘以一個矩陣呢？
  • 對角矩陣、單位矩陣、數量矩陣
    各位晚上好，今天分享的內容是一個特殊矩陣——對角矩陣，以及由此而衍生出的單位矩陣和數量矩陣。
  • No.3 矩陣乘法
    所以矩陣加法和減法我們要求兩個矩陣的維數是一摸一樣的，很簡單的推廣，沒啥好說的。下面主要聊聊矩陣乘法。「矩陣除法」（目前看來是錯誤稱呼）我們下次再聊。二、矩陣乘法            我前面舉過一個學生成績的例子。
  • (加餐)歐拉角及矩陣旋轉
    ，本文是分析微都卜勒的數學基礎，或者說研究微都卜勒的必備數學工具，為後續聊微都卜勒做鋪墊，也是就是（如何做好一款4D 高分辨毫米波雷達）中的Micro Doppler部分，咱們開始。Preliminaries剛體是指在運動中受力作用後，形狀和大小不變，而且內部各點的相對位置不變的物體。剛體是理想模型，合理的理想化模型有助於簡化複雜系統分析。分析剛體運動是後續非剛體運動分析的基礎，這是因為可以將人體等非剛體運動視作一系列剛體運動的組合，從而簡化人體等微都卜勒特徵分析。
  • 理解矩陣背後的現實意義
    比如說，在全國一般工科院系教學中應用最廣泛的同濟線性代數教材（現在到了第四版），一上來就介紹逆序數這個「前無古人，後無來者」的古怪概念，然後用逆序數給出行列式的一個極不直觀的定義，接著是一些簡直犯傻的行列式性質和習題——把這行乘一個係數加到另一行上，再把那一列減過來，折騰得那叫一個熱鬧，可就是壓根看不出這個東西有嘛用。
  • 什麼叫矩陣式大燈,矩陣led和led區別
    矩陣式大燈其實是LED大燈的升級版，燈體內部由多個LED燈按照長方形陣列排列布置。矩陣式大燈可以控制每顆LED燈珠單體進行照明，從而實現對照明角度和範圍的精確控制，是目前最為先進的前大燈系統之一。
  • 人類對抗外星人的超級武器：雷射矩陣、離子炮矩陣、電磁炮矩陣等
    不同的宇宙環境誕生出來不一樣的文明，人類的善惡定義極可能對外星人不適用。文明與文明之間最大的衝突不是利益的衝突，而是價值觀的衝突。彼之砒霜吾之蜜糖。人類會和外星人相遇嗎？人類文明其實與外星人隨時都可能相遇，或許就在下一秒。因此，人類未雨綢繆，研究和開發對抗外星人的武器極有必要。以下正在研發的超級武器，未來可能在對抗外星人入侵時產生巨大作用。