在學習高博的slam時,用的是slambook2-master這個新倉庫,在ch5/stereo這個工程運行時,做了一些更改;
工程包括如下內容(其中build是自己創建的)
根據安裝依賴庫的路徑,對CMakeLists.txt做了些更改,更改後的內容如下:
find_package(Pangolin REQUIRED)
find_package(OpenCV 3.0 REQUIRED) # add by csq,由於我裝了好幾個版本的,所以這裏指定個高版本的
include_directories(${OpenCV_INCLUDE_DIRS}) #add by csq
#添加Eigen頭文件
include_directories("/usr/include/eigen3") #add by csq ,根據你的安裝路徑來選擇
set(CMAKE_CXX_FLAGS "-std=c++11")
add_executable(stereoVision stereoVision.cpp)
target_link_libraries(stereoVision ${OpenCV_LIBS} ${Pangolin_LIBRARIES})
stereoVision.cpp的代碼分析如下:
#include <opencv2/opencv.hpp>
#include <vector>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>
//unistd.h爲Linux/Unix系統中內置頭文件,包含了許多系統服務的函數原型,例如read函數、write函數和getpid函數等。其作用相當於windows操作系統的"windows.h",是操作系統爲用戶提供的統一API接口,方便調用系統提供的一些服務。
using namespace std;
using namespace Eigen;
// 文件路徑
string left_file = "./left.png";
string right_file = "./right.png";
// 在pangolin中畫圖,已寫好,無需調整,定義繪製點雲的函數,需要傳入4維向量構成的點雲集
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;
// 基線 (就是兩個相機光軸間的距離,單位是m)
double b = 0.573;
// 讀取圖像(以灰度圖形式)
cv::Mat left = cv::imread(left_file, 0);
cv::Mat right = cv::imread(right_file, 0);
cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create( //SGBM是立體匹配算法中的半全局塊匹配,得到的視差圖相比於BM算法來說,減少了很多不準確的匹配點,尤其是在深度不連續區域,速度上SGBM要慢於BM算法;
0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32); // 神奇的參數
cv::Mat disparity_sgbm, disparity;
sgbm->compute(left, right, disparity_sgbm); //由左右視圖按照SGBM匹配方式計算得到視差圖
disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f); //將16位符號整形的視差Mat轉換爲32位浮點型Mat
//生成點雲
vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud; //定義4維形式的點雲向量容器
// 如果你的機器慢,請把後面的v++和u++改成v+=2, u+=2
for (int v = 0; v < left.rows; v++) //遍歷左視圖
for (int u = 0; u < left.cols; u++) {
if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;
Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前3維爲xyz,第4維爲歸一化後的強度值
// 根據雙目模型計算point的位置,計算的是左視圖點的相機位置
double x = (u - cx) / fx; //該公式計算的是歸一化在相機Zc=1平面的相機座標
double y = (v - cy) / fy;
double depth = fx * b / (disparity.at<float>(v, u)); //由視差,雙目的基計算像素點對應的實際距離(深度信息)
point[0] = x * depth; //由深度信息獲取真實相機座標系下的Xc
point[1] = y * depth; //由深度信息獲取真實相機座標系下的Yc
point[2] = depth; //相機座標系下的Zc
pointcloud.push_back(point); //獲得的是相機座標系下的點雲位置
}
cv::imshow("disparity", disparity / 96.0); //把視差值限定在0-96
cv::waitKey(0);
// 畫出點雲
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;
}
其中stereoSGBM的函數和參數說明如下:
static Ptr<StereoSGBM> cv::StereoSGBM::create (int minDisparity = 0,
int numDisparities = 16,
int blockSize = 3,
int P1 = 0,
int P2 = 0,
int disp12MaxDiff = 0,
int preFilterCap = 0,
int uniquenessRatio = 0,
int speckleWindowSize = 0,
int speckleRange = 0,
int mode = StereoSGBM::MODE_SGBM
)
- minDisparity:最小的視差值。
- numDisparity:視差範圍,即最大視差值和最小視差值之差,必須是16的倍數。
- blockSize:匹配塊大小(SADWindowSize),必須是大於等於1的奇數,一般爲3~11 。
- P1,P2:懲罰係數,一般:P1=8*通道數*SADWindowSize*SADWindowSize,P2=4*P1
- disp12MaxDiff :左右視差圖的最大容許差異(超過將被清零),默認爲 -1,即不執行左右視差檢查。
- preFilterCap:預濾波圖像像素的截斷值。該算法首先計算每個像素的x導數,並通過[-preFilterCap,preFilterCap]間隔剪切其值。結果值被傳遞給Birchfield-Tomasi像素成本函數。
- uniquenessRatio:視差唯一性百分比, 視差窗口範圍內最低代價是次低代價的(1 + uniquenessRatio/100)倍時,最低代價對應的視差值纔是該像素點的視差,否則該像素點的視差爲 0,通常爲5~15.
- speckleRange:視差變化閾值。
- mode:模式
實踐運行步驟如下,
#終端切到工程目錄下
cd /home/chensq/Downloads/slambook2-master/ch5/stereo
#創建build並cd
mkdir build
cd build
#編譯
cmake ..
make
在build中生成了個stereoVision的執行文件,將左右圖像拷貝進來做測試
原圖如下:
運行
./stereoVision
測試結果如下:
視差圖結果
點雲圖如下
參考: