高博SLAM基礎課第四講——相機模型

 

（1）本節內容
1、針孔相機模型   2、誤差來源——畸變 3、雙目相機模型 

（2）需要的基礎知識
單獨成章節，不需要太多基礎

（3）開發環境
編譯平台：ubuntu16.04，
編譯軟件：IDE：Clion 編譯器：Cmake 語言標准：C++11

（4）學習內容

1、針孔相機模型

小孔模型能夠把三維世界中的物體投影到一個二維成像平面。同理，可以用這個簡單的模型來解釋相機的成像模型。 

對這個簡單的針孔模型進行幾何建模。設O−x−y−zO−x−y−z為相機坐標系，習慣上讓z軸指向相機前方，x向右，y向下。O和為攝像機的光心，也是針孔模型中的針孔。現實世界的空間點P，經過小孔O投影之后，落在物理成像平面O′−x′−y′−z′上，成像點為P′。設P的坐標為[X,Y,Z]T，P′為[X′,Y′,Z′]T，並且設物理成像平面到小孔的距離為ff焦距）。那么，根據三角形相似關系，有： 

                                
其中負號表示成的像是倒立的。為了簡化模型，可以將成像平面對稱到相機前方，和三維空間點一起放在攝像機坐標系的同一側，如圖4-2中間的樣子所示。這樣可以把公式中的負號去掉，使式子更加簡潔： 

                                                 

整理得： 

                                                                              
上式描述了點P和它的像之間的空間關系。不過，在相機中，最終獲得的是一個個的像素，這需要在成像平面上對像進行采樣和量化。為了描述傳感器將感受到的光線轉換成圖像像素的過程，在物理成像平面上固定一個像素平面o−u−v。在像素平面上得到了P′的像素坐標：[u,v]T 
像素坐標系通常的定義方式是：原點o′位於圖像的左上角，u軸向右與x軸平行，v軸向下與y軸平行。像素坐標系與成像平面之間，相差了一個縮放和一個原點的平移。設像素坐標在u軸上縮放了α倍，在v上縮放了β倍。同時，原點平移了[cx,cy]T。那么，P′點的坐標與像素坐標[u,v]T的關系為：

該式中，把中間的量組成的矩陣稱為相機的內參數矩陣K。

               

K有4個未知數和相機的構造相關，fx,fy和相機的焦距，像素的大小有關；cx,cy是平移的距離，和相機成像平面的大小有關。

求解K的過程叫做相機的標定，通常認為，相機的內參在出廠之后是固定的，不會在使用過程中發生變化。有的相機生產商會告訴相機的內參，而有時需要自己確定相機的內參，也就是所謂的標定。標定算法已成熟，在網絡上可以找到大量的標定教學。

外參數

其中，p是圖像中像點的像素坐標，K是相機的內參數矩陣，P是相機坐標系下的三維點坐標。
上面推導使用的三維點坐標是在相機坐標系下的，相機坐標系並不是一個“穩定”的坐標系，其會隨着相機的移動而改變坐標的原點和各個坐標軸的方向，用該坐標系下坐標進行計算，顯然不是一個明智的選擇。需要引進一個穩定不變坐標系：世界坐標系，該坐標系是絕對不變，SLAM中的視覺里程計就是求解相機在世界坐標系下的運動軌跡。

設Pc是P在相機坐標系坐標，Pw是其在世界坐標系下的坐標，可以使用一個旋轉矩陣R和一個平移向量t，將Pc變換為Pw      

                                                       

2、畸變

為了獲得好的成像效果，在相機前方加了透鏡。透鏡的加入對成像過程中光線的傳播會產生新的影響：一是透鏡自身的形狀對光線傳播的影響，二是在機械組裝過程中，透鏡和成像平面不可能完全平行，這也會使得光線穿過透鏡投影到成像平面時的位置發生變化。 
由透鏡形狀引起的畸變稱之為徑向畸變。它們主要分為兩大類，桶形畸變和枕形畸變

切向畸變。 

                                    
平面上的任意一點p可以用笛卡爾坐標表示為[x,y]T，也可以寫成極坐標的形式[r,θ]T， 
其中r表示點p離坐標系原點的距離，θ表示和水平軸的夾角。 
對於徑向畸變，可以用一個多項式函數來描述畸變前后的坐標變化： 

                              
其中[x,y]T[x,y]T是為糾正的點的坐標，[xcorrccted,ycorrccted]T[xcorrccted,ycorrccted]T是糾正后的點的坐標，它們都是歸一化平面上的點。 
對於切向畸變，有： 

                                                                  
通過五個畸變系數找到相機坐標系中的一點在像素平面上的正確位置：
將三維空間點投影到歸一化平面。設它的歸一化坐標為[x,y]T。
對歸一化平面上的點進行徑向畸變和切向畸變糾正。 

                                       
3.將糾正后的點通過內參數矩陣投影到像素平面，得到改點在圖像上的正確位置。 

                                                                               

3、雙目相機

                              

                                          

 

轉載自https://blog.csdn.net/weixin_38593194/article/details/86348975

 作業1：

 

去畸變代碼：

#include <opencv2/opencv.hpp>
#include <string>
#include <math.h>

using namespace std;

string image_file = "../test.png";   // 請確保路徑正確

int main(int argc, char **argv) {

    // 本程序需要你自己實現去畸變部分的代碼。盡管我們可以調用OpenCV的去畸變，但自己實現一遍有助於理解。
    // 畸變參數
    double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
    // 內參
    double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;

    cv::Mat image = cv::imread(image_file,0);   // 圖像是灰度圖，CV_8UC1
    int rows = image.rows, cols = image.cols;
    cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   // 去畸變以后的圖

    // 計算去畸變后圖像的內容
    for (int v = 0; v < rows; v++)
        for (int u = 0; u < cols; u++) {

            double u_distorted = 0, v_distorted = 0;
            // TODO 按照公式，計算點(u,v)對應到畸變圖像中的坐標(u_distorted, v_distorted) (~6 lines)
            // start your code here
            //image_undistort中含有非畸變的圖像坐標
            //將image_undistort的坐標通過內參轉換到歸一化坐標系下，此時得到的歸一化坐標是對的
            //將得到的歸一化坐標系進行畸變處理
            //將畸變處理后的坐標通過內參轉換為圖像坐標系下的坐標
            //這樣就相當於是在非畸變圖像的圖像坐標和畸變圖像的圖像坐標之間建立了一個對應關系
            //相當於是非畸變圖像坐標在畸變圖像中找到了映射
            //對畸變圖像進行遍歷之后，然后賦值（一般需要線性插值，因為畸變后圖像的坐標不一定是整數的），即可得到矯正之后的圖像
            double x1,y1,x2,y2;
            x1 = (u-cx)/fx;
            y1 = (v-cy)/fy;
            double r2;
            r2 = pow(x1,2)+pow(y1,2);
            x2  = x1*(1+k1*r2+k2*pow(r2,2))+2*p1*x1*y1+p2*(r2+2*x1*x1);
            y2 = y1*(1+k1*r2+k2*pow(r2,2))+p1*(r2+2*y1*y1)+2*p2*x1*y1;

            u_distorted = fx*x2+cx;
            v_distorted = fy*y2+cy;

            // end your code here

            // 賦值 (最近鄰插值)
            if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
                image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int) u_distorted);
            } else {
                image_undistort.at<uchar>(v, u) = 0;
            }
        }

    // 畫圖去畸變后圖像
    cv::imshow("image undistorted", image_undistort);
    cv::waitKey();

    return 0;
}

1.Clion識別路徑時的當前路徑在工程文件夾下的cmaka-build-debug文件夾中

Run->Edit Configurations中修改Working directory可以修改當前路徑

2.計算的x1，y1皆為相機Z=1平面

3.要注意x和u是橫坐標，y和v是縱坐標的對應關系

結果如下

 注意到去畸變后由於插值做法使得珠子上的鐵環產生鋸齒，這個特征或許會影響特征點的魯棒性

待學習后續知識后推導

 

另外調用opencv庫的做法代碼如下

Mat imageAPI;
    Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
    cameraMatrix.at<double>(0, 0) = fx;
    cameraMatrix.at<double>(0, 1) = 0;
    cameraMatrix.at<double>(0, 2) = cx;
    cameraMatrix.at<double>(1, 1) = fy;
    cameraMatrix.at<double>(1, 2) = cy;
    cameraMatrix.at<double>(2,2)=1;
//標定矩陣
    Mat distCoeffs = Mat::zeros(5, 1, CV_64F);
    distCoeffs.at<double>(0, 0) = k1;
    distCoeffs.at<double>(1, 0) = k2;
    distCoeffs.at<double>(2, 0) = p1;
    distCoeffs.at<double>(3, 0) = p2;
    distCoeffs.at<double>(4, 0) = 0;
//畸變向量
    Mat view, rview, map1, map2;
    Size imageSize;
    imageSize = image.size();
    initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
                            getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0),
                            imageSize, CV_16SC2, map1, map2);
    remap(image, imageAPI, map1, map2, INTER_LINEAR);

    // 畫圖去畸變后圖像
    cv::imshow("imageAPI", imageAPI);
    cv::waitKey();

此輸出為

以后需要用再深究，花時間有點久了。

 

 

作業2：

 

 

 代碼如下：

#include <opencv2/opencv.hpp>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>

using namespace std;
using namespace Eigen;

// 文件路徑，如果不對，請調整
string left_file = "/home/steve/left.png";
string right_file = "/home/steve/right.png";
string disparity_file = "/home/steve/disparity.png";

// 在panglin中畫圖，已寫好，無需調整
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);

int main(int argc, char **argv) {

    // 內參
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // 間距
    double b = 0.573;

    // 讀取圖像
    cv::Mat left = cv::imread(left_file, 0);
    cv::Mat right = cv::imread(right_file, 0);
    cv::Mat disparity = cv::imread(disparity_file, 0); // disparty 為CV_8U,單位為像素

    // 生成點雲
    vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;

    // TODO 根據雙目模型計算點雲
    // 如果你的機器慢，請把后面的v++和u++改成v+=2, u+=2
    for (int v = 0; v < left.rows; v+=1)
        for (int u = 0; u < left.cols; u+=1) {

            Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三維為xyz,第四維為顏色

            // start your code here (~6 lines)
            // 根據雙目模型計算 point 的位置
            // start your code here (~6 lines) // 根據雙目模型計算 point 的位置
            unsigned char d=disparity.ptr<unsigned char>(v)[u];

            point[2]=(fx*b)/(double)d;
            point[1]=(v-cy)*point[2]/fy;
            point[0]=(u-cx)*point[2]/fx;
            pointcloud.push_back(point);

            // end your code here
        }

    // 畫出點雲
    showPointCloud(pointcloud);
    return 0;
}

void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {

    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) {
            glColor3f(p[3], p[3], p[3]);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
    return;
}

效果：

 

 自寫部分：

unsigned char d=disparity.ptr<unsigned char>(v)[u];
            point[2]=(fx*b)/(double)d;
            point[1]=(v-cy)*point[2]/fy;
            point[0]=(u-cx)*point[2]/fx;
            pointcloud.push_back(point);

https://blog.csdn.net/fb_941219/article/details/89716815#2Disparity_71
有詳解

此處讀取視差圖要用uchar格式，用short讀會出問題
由視差求深度的公式為depth=f×baseline/disparity 此處f和d的單位都是像素，除得用米表示的深度
不過由於此處x，y都是按比例縮放的，只要正確讀出了disparity，在點雲中顯示都是一樣的（范圍不同）

希望知道怎么用這些庫啊..

PS：

vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d>>;

其實上述的這段代碼才是標准的定義容器方法，只是我們一般情況下定義容器的元素都是C++中的類型，所以可以省略，這是因為在C++11標准中，aligned_allocator管理C++中的各種數據類型的內存方法是一樣的，可以不需要着重寫出來。但是在Eigen管理內存和C++11中的方法是不一樣的，所以需要單獨強調元素的內存分配和管理。