個人博客：http://www.chenjianqu.com/
原文鏈接：http://www.chenjianqu.com/show-80.html
本文是讀高翔大佬的<視覺SLAM14講>的筆記，準備開始入坑了。。。
針孔相機模型
大部分常見的相機都可以抽象爲針孔模型：
其中P點是三維空間中的一點，P’點是P在圖片上的投影點，O是相機座標系的原點，O’是物理成像平面的原點。可得Z/f=X/X’=Y/Y’，即：X’=f*X/Z，Y’=f*Y/Z，其中[X,Y,Z]是P點在相機座標系的座標。
像素座標系o-u-v在物理成像平面上。原點位於圖像的左上角，u軸向右與x軸平行，v軸向下與y軸平行。像素座標系與物理成像座標系之間相差一個縮放和一個原點的平移。設像素座標在u軸上縮放了α倍，在v軸上縮放了b倍，同時原點平移了[cx,cy]，則P’的物理座標與像素座標的關係爲：u=α*X'+cx, v=β*Y'+cy。代入X'和Y'，並把α*f合併爲fx，β*f合併爲fy，得：
其中，f的單位爲米，α, β的單位爲像素/米，所以fx, fy和cx, cy的單位爲像素。寫成矩陣形式：
或：
其中K稱爲相機的內參數矩陣(Camera Intrinsics)。內參通常在出廠後是固定的，有時需要自己確定內參，也就是標定。
點P在世界座標系中的座標爲Pw，在相機座標系中的座標爲P，P是Pw根據相機當前位姿從世界座標系變換到相機座標系下的結果。相機的位姿由相機相對於世界座標的旋轉矩陣R和平移向量t來描述，則有：
T是R和t構成的齊次座標，先與Pw相乘，再與K相乘。該公式描述了P點的世界座標到像素座標的投影關係，其中相機的位姿R,t又被稱爲外參數(Camera Extrinsics)。
換個角度，也可以先把P點從世界座標系投影到相機座標系，再投影到像素座標系。其中投影到相機座標系的時候，可以去掉座標的最後一維，也就是深度維，把該維度置爲1。得到P點在相機歸一化平面上的投影：
歸一化座標可看成相機前方z = 1 處的平面上的一個點，這個 z = 1 平面也稱爲歸一化平面。歸一化座標再左乘內參就得到了像素座標，所以可以把像素座標 [u, v]看成對歸一化平面上的點進行量化測量的結果，即像素座標系也可以放在歸一化平面上。
從該過程可知，空間點的深度信息在投影過程中被丟失了，單目視覺中沒法得到像素點的深度值。如下所示：
雙目相機模型
雙目相機一般由水平放置的左眼相機和右眼相機組成，可以把兩個相機都看作針孔相機。因爲是水平放置的，意味着兩個相機的光圈中心都位於x軸上，兩者之間的距離稱爲雙目相機的基線(Baseline)。雙目相機的成像模型如下：
根據三角形的相似性，有：
z是相機座標系中P點的z維數值(即P點的深度)，f是相機的焦距，uL和uR是像素座標系(這裏把成像平面放在相機前面，等價與放在後面的情況)的座標值，d爲左右圖的橫座標之差，稱爲視差(Disparity)。
由z=fb/d可知，根據視差，可以估計像素與相機之間的距離。視差與距離成反比：視差越大，距離越近。由於視差最小爲一個像素，於是雙目的深度存在一個理論上的最大值，由 fb 確定。當基線越長時，雙目能測到的最大距離就會越遠。
雖然由視差計算深度的公式很簡潔，但視差d本身的計算卻比較困難，需要確切地知道左眼圖像某個像素出現在右眼圖像的哪一個位置(即對應關係)。常用的計算視差的算法有SAD,GC,SGBM,DP,BM等，比較如下
SGBM
semi-global matching(SGM)是一種用於計算雙目視覺中視差（disparity）的半全局匹配算法，在OpenCV中的實現爲semi-global block matching(SGBM)。https://blog.csdn.net/A_L_A_N/article/details/81490043
OpenCV實例
因此跟Anaconda衝突了，PCL死活編譯不過，因此這裏運行高博給的代碼，他是用Pangolin顯示點雲：
CMakeLists.txt
cmake_minimum_required(VERSION 2.6)
project(imagebinoculartest)
# 添加c++ 11標準支持
set( CMAKE_CXX_FLAGS "-std=c++11" )
find_package( OpenCV 3 REQUIRED )
find_package( Pangolin )
include_directories( ${OpenCV_INCLUDE_DIRS} )
include_directories("/usr/include/eigen3")
include_directories( ${Pangolin_INCLUDE_DIRS} )
add_executable(imagebinoculartest main.cpp)
target_link_libraries(imagebinoculartest ${OpenCV_LIBS})
target_link_libraries( imagebinoculartest ${Pangolin_LIBRARIES} )
install(TARGETS imagebinoculartest RUNTIME DESTINATION bin)
main.cpp
#include <iostream>
#include <chrono>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Core>
#include<opencv2/calib3d/calib3d.hpp>
#include <pangolin/pangolin.h>
using namespace std;
using namespace Eigen;
using namespace cv;
//高博用pangolin中顯示點雲
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;// 基線
cout << "OpenCV version : " << CV_VERSION << endl;
Mat leftImg=imread("left.png",0);
Mat rightImg=imread("right.png",0);
imshow ( "leftImg", leftImg);
imshow ( "rightImg", rightImg);
waitKey ( 0 );
//OpenCV實現的SGBM立體匹配算法
Ptr<StereoSGBM> sgbm = StereoSGBM::create(
0,//minDisparity 最小視差
96, //numDisparities 視差搜索範圍，值必需爲16的整數倍。最大搜索邊界=最小視差+視差搜索範圍
9, //blockSize 塊大小
//8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
8 * 9 * 9, //P1 控制視差變化平滑性的參數。P1、P2的值越大，視差越平滑。P1是相鄰像素點視差增/減 1 時的懲罰係數；P2是相鄰像素點視差變化值大於1時的懲罰係數。P2必須大於P1。
//32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize
32 * 9 * 9, //P2
1, //disp12MaxDiff 左右一致性檢測最大容許誤差閾值。
63, //preFilterCap,預處理時映射參數
10, //uniquenessRatio 唯一性檢測參數，
100, //speckleWindowSize 視差連通區域像素點個數的大小。對於每一個視差點，當其連通區域的像素點個數小於speckleWindowSize時，認爲該視差值無效，是噪點。
32//speckleRange 視差連通條件，在計算一個視差點的連通區域時，當下一個像素點視差變化絕對值大於speckleRange就認爲下一個視差像素點和當前視差像素點是不連通的。
);
Mat disparity_sgbm, disparity;
sgbm->compute(leftImg, rightImg, disparity_sgbm); //計算視差圖
disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);//得到視差圖
cv::imshow("disparity", disparity / 96.0);
cv::waitKey(0);
// 生成點雲
vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;
// 如果機器慢，把後面的v++和u++改成v+=2, u+=2
for (int v = 0; v < leftImg.rows; v++)
for (int u = 0; u < leftImg.cols; u++) {
if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0)
continue;
Vector4d point(0, 0, 0, leftImg.at<uchar>(v, u) / 255.0); // 前三維爲xyz,第四維爲顏色
// 根據雙目模型計算 point 的位置
double x = (u - cx) / fx;
double y = (v - cy) / fy;
double depth = fx * b/(disparity.at<float>(v, u));
point[0] = x * depth;
point[1] = y * depth;
point[2] = depth;
pointcloud.push_back(point);
}
cv::imshow("disparity", disparity / 96.0);
cv::waitKey(0);
// 畫出點雲
showPointCloud(pointcloud);
return 0;
}
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud)
{
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;
}
原始左右兩圖：
SGBM計算得到的視差圖：
根據視差圖得到的點雲圖：
參考文獻
[0]高翔.視覺SLAM 14講