此博客記錄我的slam學習之旅，近幾天目標為實現基於RGB-D SLAM的視覺里程計。主要參考資料為高博的《視覺SLAM十四講：從理論到實踐》及高博的博客https://www.cnblogs.com/gaoxiang12/。
首先實現一個不考慮任何實際問題的視覺里程計，根據RGB圖像及對應的深度圖像恢復出相鄰兩幀圖像間的R,T（也就是運動相機的位置姿態），幀與幀之間的運動關系最后構成相機的運動軌跡。大致思路分為如下幾個步驟：
（1）在圖像上選取特征點，進行圖像間的特征點匹配。
（2）根據特征匹配的結果（3D-3D），利用ICP算法恢復出相機的位置姿態（以第一幀的相機坐標系為參考坐標系）
（3）兩兩幀間的運動估計完成，再進行幀與幀之間的連接，最后連接成整體的軌跡了。
（ps:把視覺里程計寫的這么簡單也是沒誰了。。。。。實際的情況比這復雜很多）
補充一點基本數學知識：（這里只考慮小孔成像模型，實際還有幾種其他攝像機模型）
（本人使用opencv 3.1，ubuntu 16.04（在虛擬機上運行））
相機與圖像的坐標系
（1）相機坐標系
（2）圖像坐標系
（3）像素坐標系
由圖可知，設空間點P在相機坐標系的坐標為P[x,y,z]，通過相機光心投影到圖像坐標系中P/(x/,y/)，根據相似三角形原理（如上圖所示）可知，
（ps:由於第一次寫博客，不會使用公式編輯插件，所以在草稿本上寫放的照片）
為了簡化模型，將成像平面放到相機前方，可以得到對應公式，不過在相機圖像中，我們得到的是像素，我們設物理成像平面坐標系上固定着一個像素坐標系，p/對應像素坐標p（u,v）
圖像成像坐標系與像素坐標系之間相差了一個縮放和平移，設在u軸上被縮放了a倍，按原點平移了cx，在v軸縮放了B倍，按原點平移了cy，即可得上述公式
為了易於表達，將左側公式寫成齊次坐標形式，其中K為相機內參矩陣，一般工廠會標定好相機內參，但很多時候需要我們進行標定，標定工具主要有基於Opencv和matlab的，目前已經很成熟了
有相機內參自然有相機外參，上面假定的空間點p的坐標是在相機坐標系下的，那么如何根據世界坐標系下的p點計算得到像素坐標下對應的點？
因為都是齊次坐標，所以可以寫成上述公式。
下面貼一段代碼表達像素坐標與相機坐標之間的關系：
以下是2d-3d的轉換代碼：
添加CMakeLists.txt
#設定最小版本 CMAKE_MINIMUM_REQUIRED(VERSION 2.8) #設定工程名 PROJECT(2D) # 添加c++ 11標准支持 set( CMAKE_CXX_FLAGS "-std=c++11" ) #找到opencv庫 FIND_PACKAGE(OpenCV REQUIRED) # 添加頭文件 include_directories( ${OpenCV_INCLUDE_DIRS} ) ADD_EXECUTABLE(2D 2d.cpp) # 鏈接OpenCV庫 target_link_libraries( 2D ${OpenCV_LIBS}
以下是2d轉換為3d坐標的代碼
// // Created by xl on 18-5-4. //此程序為將圖像像素坐標轉為相機坐標系，原圖像為一張RGB圖像和一張對應的深度圖 //opencv庫 #include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp> //c++庫 #include <iostream> using namespace std; using namespace cv; int main(int argc,char **argv) { Mat rgb=imread("rgb.png",0); Mat depth=imread("depth.png",-1);//-1表示對原數據不做任何修改 double scale=1000;//scale表示深度圖的縮放因子，一般為1000 /*Mat K; K=(Mat_<double>(3,3)<<518.0,0,325.5,0,518.0,253.5,0,0,1);*///定義相機內參K,也可以采用這種方式 // 相機內參 const double camera_factor = 1000; const double camera_cx = 325.5; const double camera_cy = 253.5; const double camera_fx = 518.0; const double camera_fy = 519.0; vector<Point3f> pts_3d;//定義三維點 for(int m=0;m<depth.rows;m++) { for(int n=0;n<depth.cols;n++){ ushort d=depth.ptr<unsigned short>(m)[n]; if(d==0) continue;//如果d=0，則被認為是壞點 double dd=d/scale; pts_3d.push_back(Point3f((m-camera_cx)*dd/camera_fx,(n-camera_cy)*dd/camera_fy,dd)); } } }
既然講到圖像了，那就順便提一下圖像畸變吧。
為了呈現更好的成像效果，我們會在相機前方加上透鏡，透鏡的加入會對成像過程中光線的傳播產生新的影響。
畸變的形式一般為兩種：
(1)徑向畸變：由透鏡形狀引起的畸變，徑向畸變又可以分為枕形畸變和桶形畸變。
(2)切向畸變：在組裝過程中，透鏡和成像平面不能嚴格平行產生切向畸變
對於切向畸變，無論哪個形式，都可以用一個多項式函數來描述畸變前后的變化，可以用與距中心的距離有關的二次及高次多項式函數進行糾正。
對於切向畸變，也可以使用兩個參數來糾正。(具體公式見下圖)
注意：x,y為歸一化平面的坐標，r也是距歸一化中心點的距離。
所以給出一個空間點位置P(X,Y,Z),我們可以求出其在帶有畸變的相機上像素坐標。
（ps:對了，之前沒有講到歸一化坐標，這里的歸一化坐標其實就是相機坐標系下的坐標值（x,y,z）轉換到z=1的平面上，原來的坐標轉為（x/z,y/z,1））
接下來到了我們的實踐環節了，給你一張帶有畸變的圖像，如何把它糾正轉為去畸變的照片？？
問題描述：
input:畸變后的圖像（因為相機原因，可能拍出來的照片里面的物體是斜的），上面提到的畸變參數
output:畸變前的圖像。
（ps:最開始我接觸這個問題的時候，當時也不明白該怎么求，后來看了網上的博客，才弄懂了。）
其實這個問題的核心是畸變后的坐標與畸變前的坐標，它們兩處坐標的灰度值是相同的，這里要求出畸變前的圖像，其實就是求出畸變前圖像的一個點坐標p，對應的畸變圖像的坐標是多少，找到p點對應到畸變圖像上的坐標值，再把畸變圖像的坐標值的灰度值賦給畸變前的坐標p，所有的圖像是一個個像素組成的，遍歷所有圖像這就可以了。。。所以這個問題的求解可以分為以下步驟
(注意：這里還有一個隱藏問題，像素點（u,v）經過畸變后在畸變圖像的坐標值不一定為整數，但是在像素坐標里一定要是整數的啊，比如在原始圖像（1，1）畸變后就可能變成了（1.2，1.3）,這樣顯然是不行的，這就需要對畸變坐標作插值處理，我們這里先不討論插值的問題，對畸變坐標取整進行（ps:這樣真的是太暴力了啊 。。。。）)
（還有個問題：我們這里討論的是灰度圖像，當然也可以考慮RGB圖像，其實原理是一樣的，只要每個坐標計算三個RGB值就行了，這里以灰度圖為例）
下面就是代碼示例了：
#include <iostream> #include <opencv2/core/core.hpp> #include <opencv2/features2d/features2d.hpp> #include <opencv2/highgui/highgui.hpp> #include <opencv2/calib3d/calib3d.hpp> usingnamespace std; usingnamespace cv; int main(int argc, char **argv) { // 畸變參數,這里只考慮k1,k2,這里需要你知道參數。 double k1,k2,p1,p2; //相機內參矩陣 double fx,fy,cx,cy; Mat K; K=(Mat_double<3,3><<fx,0,cx,0,fy,cy,0,0,1); Mat image = imread(“test.png”,0); // 這里填寫你的路徑，0表示將圖像轉換為灰度圖 Mat image1 = Mat(image.rows, image.cols, CV_8UC1); // 設置去畸變以后的圖 // u,v為畸變前圖像的像素坐標,v為列，u為行 for (int v= 0; v < image.rows; v++) for (int u = 0; u < image.cols; u++) { double x=(u-cx)/fx; double y=(v-cy)/fy;//這里的x,y為歸一化后的坐標 double r=pow(x,2)+pow(y,2);//r為距中心點的距離的平方，之所以是平方，是因為方便，沒有調用開根號，注意也是歸一化平面上的 double x1=x*(1+k1*r+k2*pow(r,2))+2*p1*x*y+p2*(r+2*x*x); double y1=y*(1+k1*r+k2*pow(r,2))+p1*(r+2*y*y)+2*p2*x*y;//x1,y1為畸變圖像上的歸一化坐標 double u1=fx*x1+cx; double v1=fy*y1+cy;//u1，v1為畸變圖像上的像素坐標 // 最后一步灰度值相等，而且需要對畸變圖像的像素坐標取整，這里的方法很粗暴，直接int if (u1= 0 && v1>=0 && u1< image.cols && v1<image.rows) { image1.at<uchar>(v, u) = image.at<uchar>((int) v1, (int) u1); } else { image1.at<uchar>(v, u) = 255;//對於超出坐標范圍，賦灰度值為255 } } // 畫圖去畸變后圖像 imshow("image1", image1); imshow("image ", image); cv::waitKey(); return0; }
(ps:代碼寫得可能不夠規范，請原諒啊，哈哈，因為中英文字符問題調了半天)
好了，這次主要介紹相機圖像的問題了，下一節將記錄特征點檢測匹配的問題了。。
如果有人看到我的博客，歡迎大家一起交流啊，，，，（寫的這么爛估計沒有人看吧。。。。。）
我的郵箱：1439741774@qq.com18:21:24