文章目錄

• 5. 相機與模型
• • 5.1 相機模型
  • 5.2 圖像
  • 5.3 實踐
  • • 5.3.1 Open CV的基本使用方法
    • 5.3.2 圖像去畸變
  • 5.4 3D視覺
  • • 5.4.1 雙目視覺
    • 5.4.2 RGB-D視覺
  • 5.5 利用 KDevelop IDE編譯器 編譯執行文件

5. 相機與模型

5.1 相機模型

理論參考博客:

1.《視覺SLAM十四講》相機位姿與相機外參的區別與聯系

2.《視覺SLAM十四講》學習筆記：第5講相機與圖像

5.2 圖像

理論參考博客:

1.視覺SLAM十四講學習筆記-第五講-圖像和實踐

5.3 實踐

5.3.1 Open CV的基本使用方法

例程中演示了對 圖像讀取,顯示,像素遍歷,復制,賦值等.

代碼及注釋如下:

#include <iostream> #include <chrono>//時間相關的庫using namespace std;#include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp>int main(int argc, char **argv) {// 讀取argv[1]指定的圖像cv::Mat image;image = cv::imread(argv[1]); //cv::imread函數讀取指定路徑下的圖像// 判斷圖像文件是否正確讀取if (image.data == nullptr) { //數據不存在,可能是文件不存在cerr << "文件" << argv[1] << "不存在." << endl;return 0;}// 文件順利讀取, 首先輸出一些基本信息cout << "圖像寬為" << image.cols << ",高為" << image.rows << ",通道數為" << image.channels() << endl;cv::imshow("image", image); // 用cv::imshow顯示圖像cv::waitKey(0); // 暫停程序,等待一個按鍵輸入// 判斷image的類型if (image.type() != CV_8UC1 && image.type() != CV_8UC3) {//單通道灰度,三通道彩色// 圖像類型不符合要求cout << "請輸入一張彩色圖或灰度圖." << endl;return 0;}// 遍歷圖像, 請注意以下遍歷方式亦可使用于隨機像素訪問// 使用 std::chrono 來給算法計時chrono::steady_clock::time_point t1 = chrono::steady_clock::now();for (size_t y = 0; y < image.rows; y++) { //高對應行// 用cv::Mat::ptr獲得圖像的行指針unsigned char *row_ptr = image.ptr<unsigned char>(y); // row_ptr是第y行的頭指針for (size_t x = 0; x < image.cols; x++) { //寬對應行// 訪問位于 x,y 處的像素unsigned char *data_ptr = &row_ptr[x * image.channels()]; // data_ptr 指向待訪問的像素數據// 輸出該像素的每個通道,如果是灰度圖就只有一個通道for (int c = 0; c != image.channels(); c++) {unsigned char data = data_ptr[c]; // data為I(x,y)第c個通道的值}}}chrono::steady_clock::time_point t2 = chrono::steady_clock::now();chrono::duration<double> time_used = chrono::duration_cast < chrono::duration < double >> (t2 - t1);cout << "遍歷圖像用時：" << time_used.count() << " 秒。" << endl;// 關于 cv::Mat 的拷貝// 直接賦值并不會拷貝數據cv::Mat image_another = image;// 修改 image_another 會導致 image 發生變化image_another(cv::Rect(0, 0, 100, 100)).setTo(0); // 將左上角100*100的塊置零,左上角100*100變黑色cv::imshow("image", image);cv::waitKey(0);// 使用clone函數來拷貝數據cv::Mat image_clone = image.clone();image_clone(cv::Rect(0, 0, 100, 100)).setTo(255);//左上角100*100變白色cv::imshow("image", image);cv::imshow("image_clone", image_clone);cv::waitKey(0); // 暫停程序,等待一個按鍵輸入// 對于圖像還有很多基本的操作,如剪切,旋轉,縮放等,限于篇幅就不一一介紹了,請參看OpenCV官方文檔查詢每個函數的調用方法.cv::destroyAllWindows();//關閉所有打開窗口return 0; }

編譯運行,執行如下代碼,(PS:圖片存儲位置需要替換!)

./imageBasics/imageBasics /home/lmf37/桌面/slambook2/ch5/imageBasics/ubuntu.png

執行結果如下:
(除了可以看到一張圖片,還輸出了圖片的寬度\高度信息)

5.3.2 圖像去畸變

由透鏡形狀引起的畸變叫做徑向畸變，包括桶形畸變和枕形畸變
圖像中一條直線，往里彎是桶形，往外彎是枕形【但個人習慣感覺桶形畸變是鼓起來，枕形相反,可不過也不用糾結,桶和枕名字取得很形象,遇到看圖就明白了?】
組裝過程導致的成像平面與透鏡不平行，帶來的是切向畸變

去畸變的思路是這樣的：

創建一個和原來一樣大的圖片區域 遍歷這個新圖片的所有像素（u v） 通過畸變參數，計算(u,v)處畸變后的坐標 把原圖上的畸變后的坐標處的像素值賦值給新圖片（涉及到插值） 遍歷完成后，新圖片就是去畸變的了

編譯之后直接運行就好，注意修改代碼中的圖片路徑
（與上邊的實驗不同，這里使用的路徑是在程序里寫好的）

代碼及注釋如下:

#include <opencv2/opencv.hpp> #include <string>using namespace std;string image_file = "/home/lmf37/桌面/slambook2/ch5/imageBasics/distorted.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_8UC1int 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++) {// 按照公式，計算點(u,v)對應到畸變圖像中的坐標(u_distorted, v_distorted)double x = (u - cx) / fx, y = (v - cy) / fy;double r = sqrt(x * x + y * y);double x_distorted = x * (1 + k1 * r * r + k2 * r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);double y_distorted = y * (1 + k1 * r * r + k2 * r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;double u_distorted = fx * x_distorted + cx;double v_distorted = fy * y_distorted + cy;// 賦值 (最近鄰插值)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("distorted", image);cv::imshow("undistorted", image_undistort);cv::waitKey();return 0; }

**但是!!!**,不幸的報錯了?,之前調試是通過的,不知道為什么又報錯了,錯誤如下:

OpenCV Error: Assertion failed (size.width>0 && size.height>0) in imshow, file /home/lmf37/SLAM/opencv-3.4.0/modules/highgui/src/window.cpp, line 331 terminate called after throwing an instance of 'cv::Exception'what(): /home/lmf37/SLAM/opencv-3.4.0/modules/highgui/src/window.cpp:331: error: (-215) size.width>0 && size.height>0 in function imshow

檢查了一下是上面路徑寫的不對(尷尬,自己強調的事情,轉身又掉坑里,不熟練掌握~)

//錯誤 string image_file = "./home/lmf37/桌面/slambook2/ch5/imageBasics/distorted.png/distorted.png"; //正確 string image_file = "/home/lmf37/桌面/slambook2/ch5/imageBasics/distorted.png";

執行結果如下:

5.4 3D視覺

5.4.1 雙目視覺

代碼及注釋如下:

#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 = "/home/lmf37/桌面/slambook2/ch5/stereo/left.png"; string right_file = "../stereo/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+=2for (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); // 前三維為xyz,第四維為顏色// 根據雙目模型計算 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; //由深度信息獲取真實相機坐標系下的Xcpoint[1] = y * depth; //由深度信息獲取真實相機坐標系下的Ycpoint[2] = depth; //相機坐標系下的Zcpointcloud.push_back(point); //獲得的是相機坐標系下的點云位置}cv::imshow("disparity", disparity / 96.0); //把視差值限定在0-96cv::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) { //pointcloud容器中的每一個元素從前往后枚舉出來，并用 p 來表示glColor3f(p[3], p[3], p[3]); //點的顏色glVertex3d(p[0], p[1], p[2]); //點的相機坐標}glEnd();pangolin::FinishFrame();usleep(5000); // sleep 5 ms}return; }

執行結果如下:😦注意第一行圖是像差圖,點擊任意鍵盤按鍵可顯示點云圖)😃

**插播一條小樂趣**
:)這個符號可以顯示 😃 :( 可以顯示 😦 ,碼字の小樂趣

還有這張圖有種莫名的美感,!!! 今晩（こんばん）の月（げつ）は綺麗（きれい）ですね :)
!!! 注意 可以看到相差圖中左邊有一條黑色,這是因為,左側相機看到了一部分右側相機未看到的內容,所以對應的視差是空的.😃

5.4.2 RGB-D視覺

color/有5張RGB圖,depth/有5張對應的深度圖,pose.txt文件給出了5張圖像的相機外參位姿(Twc),為平移向量加旋轉四元數:

本節代碼主要從完成兩件事:

1. 根據相機內參計算一對RGB-D圖像對應的點云
2. 根據各張圖的相機位姿(即外參),講點云加一起,組成地圖

代碼及注釋如下:

#include <iostream> #include <fstream> #include <opencv2/opencv.hpp> #include <boost/format.hpp> // for formating strings #include <sophus/se3.h> #include <pangolin/pangolin.h>using namespace std; typedef vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> TrajectoryType; typedef Eigen::Matrix<double, 6, 1> Vector6d;// 在pangolin中畫圖，已寫好，無需調整 void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud);int main(int argc, char **argv) {vector<cv::Mat> colorImgs, depthImgs; // 彩色圖和深度圖TrajectoryType poses; // 相機位姿ifstream fin("/home/lmf37/桌面/slambook2/ch5/rgbd/pose.txt");if (!fin) {cerr << "請在有pose.txt的目錄下運行此程序" << endl;return 1;}// cv::imread(string,flag);// MREAD_UNCHANGED(-1) ：不進行轉化，比如保存為了16位的圖片，讀取出來仍然為16位。// IMREAD_GRAYSCALE(0) ：進行轉化為灰度圖，比如保存為了16位的圖片，讀取出來為8位，類型為CV_8UC1。// IMREAD_COLOR(1) ：進行轉化為三通道圖像。// IMREAD_ANYDEPTH(2) ：如果圖像深度為16位則讀出為16位，32位則讀出為32位，其余的轉化為8位。for (int i = 0; i < 5; i++) {boost::format fmt("/home/lmf37/桌面/slambook2/ch5/rgbd/%s/%d.%s"); //圖像文件格式colorImgs.push_back(cv::imread((fmt % "color" % (i + 1) % "png").str()));depthImgs.push_back(cv::imread((fmt % "depth" % (i + 1) % "pgm").str(), -1)); // 使用-1讀取原始圖像// 定義一個7個變量的數組并初始化為0，然后定義一引用，一個for循環，讓for循環遍歷data中的每一元素d,并給每個元素賦位姿里的值。double data[7] = {0};for (auto &d:data)fin >> d;Sophus::SE3 pose(Eigen::Quaterniond(data[6], data[3], data[4], data[5]),Eigen::Vector3d(data[0], data[1], data[2]));poses.push_back(pose);}// 計算點云并拼接// 相機內參 double cx = 325.5;double cy = 253.5;double fx = 518.0;double fy = 519.0;double depthScale = 1000.0;vector<Vector6d, Eigen::aligned_allocator<Vector6d>> pointcloud;pointcloud.reserve(1000000);for (int i = 0; i < 5; i++) {cout << "轉換圖像中: " << i + 1 << endl;cv::Mat color = colorImgs[i];cv::Mat depth = depthImgs[i];Sophus::SE3 T = poses[i];for (int v = 0; v < color.rows; v++) // 每一行for (int u = 0; u < color.cols; u++) { // 每一列unsigned int d = depth.ptr<unsigned short>(v)[u]; // 深度值 // 獲取深度圖中像素點的深度if (d == 0) continue; // 如果為0，說明沒有測量到，那么跳過此點Eigen::Vector3d point; // 相機坐標系下的三維點point[2] = double(d) / depthScale; // depthScale是RGBD相機的參數嗎？？？point[0] = (u - cx) * point[2] / fx; // 相機成像模型（相機坐標下一點轉換到像素面），這里反過來就是已知深度和像素坐標，反推出相機坐標系下的x和ypoint[1] = (v - cy) * point[2] / fy;Eigen::Vector3d pointWorld = T * point;Vector6d p;p.head<3>() = pointWorld;// 賦值圖像中某個像素點的rgb值（注意： opencv中圖像顏色為BGR，取相應值的時候要注意順序）p[5] = color.data[v * color.step + u * color.channels()]; // bluep[4] = color.data[v * color.step + u * color.channels() + 1]; // greenp[3] = color.data[v * color.step + u * color.channels() + 2]; // redpointcloud.push_back(p);}}cout << "點云共有" << pointcloud.size() << "個點." << endl;showPointCloud(pointcloud);return 0; }void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &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) {glColor3d(p[3] / 255.0, p[4] / 255.0, p[5] / 255.0);glVertex3d(p[0], p[1], p[2]);}glEnd();pangolin::FinishFrame();usleep(5000); // sleep 5 ms}return; }

執行結果如下:
(記得修改文件位置)

PS:可以用鼠標平移旋轉點云

5.5 利用 KDevelop IDE編譯器 編譯執行文件

1.點擊構建
其中運行第一個 open CV 讀文件需注意,要將文件名參數,和位置傳入,具體如下:
程序正確運行

要換不同的程序,可以在運行–>配置啟動-->當前啟動配置 進行切換即可,然后點擊調試即可得到結果
例如切換RGB-D程序

總結

以上是生活随笔為你收集整理的视觉SLAM十四讲学习笔记——ch5 相机与图像的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。