ch8-視覺里程計2
主要目標：1.理解光流法跟蹤特征點的原理
2.理解直接法是如何估計相機位姿的
3.使用g2o進行直接法的計算
本講我們將介紹直接法的原理，並利用g2o實現直接法中的一些核心算法。
特征點法的缺點：
1）關鍵點的提取與描述子的計算非常耗時。SIFT無法在CPU上實時計算，而即使是相對較快的ORB也要將近20ms的計算。對於30ms/幀運行的SLAM系統來說，一大半時間都花在了計算特征點上。
2）使用特征點時，忽略了除特征點以外的所有信息。一副圖像有幾十萬個像素，而特征點只有幾百個。只使用特征點丟失了大部分可能有用的圖像信息。
3）如果相機運動到特征缺失的地方，比如說一堵白牆，或者一個空盪盪的走廊，這些地方往往沒有明顯的紋理信息。從而特征點數量也會明顯減少，以至於我們可能就找不到足夠的匹配點來計算相機的運動。
相應的解決方法：
1）保留特征點，但只計算關鍵點，不計算描述子。同時，使用光流法（Optical Flow）來跟蹤特征點的運動。這樣可回避計算和匹配描述子帶來的時間，但是光流本身也需要一定時間。（這種方法仍然使用特征點，只是把匹配描述子替換成了光流跟蹤，估計相機運動時仍然使用對極幾何、PnP或ICP算法。）實踐見下。
（接下來的兩種算法，是根據圖像的像素灰度信息來計算相機運動，稱之為直接法）
2）只計算關鍵點，不計算描述子。同時，使用直接法（Direct Method）來計算特征點在下一時刻圖像中的位置。這樣也能跳過描述子的計算過程，並且直接法的計算更加簡單。（稀疏直接法）
3）既不計算關鍵點，也不計算描述子，而是根據像素灰度值的差異，直接計算相機運動。（半稠密 and 稠密）
特征點法 VS 直接法
1）特征點法計算量較大（計算描述子）
2）僅提取圖像上的特征點會浪費了圖像上大部分可能有用的信息（一副圖像有幾十萬個像素，而特征點只有幾百個）
3）對於特征缺失的場景，特征點法失效
4）在應用場合上的限定：特征點法只能重構稀疏特征點（稀疏地圖），而直接法可以恢復半稠密或甚至是稠密的地圖。
5）SLAM求解優化過程中主要優化點的特征和位置匹配代價：在特征法中點的特征匹配由特征描述子確定，求解只優化點特征匹配狀態下的位置代價；直接法則是對點的特征（灰度）和位置同時進行優化，是更加end2end的方法，其優勢在於避免了絕對特征匹配帶來的局部最優問題，同時也省去了特征描述的開銷。
6）求解方法不同：
用特征點法估計相機運動時，把特征點看作固定在三維空間的不動點。根據這些特征點在相機中的投影位置，通過最小化重投影誤差（Reprojection error）(因為通過匹配描述子知道了空間點在兩個相機中投影后的像素位置，所以可以計算重投影的位置)來優化相機運動。因為在這個過程中，我們需要精確的知道空間點在兩個相機中投影后的像素位置，所以就要對匹配或跟蹤特征。而計算、匹配特征需要付出大量的計算量。
在直接法中，我們並不知道點與點之間的對應關系（第二個相機中的哪一個投影點對應第一個相機中的投影點，直接法的思路是根據當前相機的位姿估計值來尋找第二個i相機中的投影點的位置），而是通過最小化光度誤差（Photometric error）來求得它們。
相對於特征點法，直接法的優點：
首先要知道直接法的存在就是為了克服特征點法的缺點。1）直接法根據像素的亮度信息估計相機的運動，故而可以完全不用計算關鍵點和描述子。這樣，既避免了特征點的計算時間，又避免了特征缺失的情況。2）特征點法只能重構稀疏特征點（稀疏地圖），而直接法還可以恢復稠密或半稠密結構的地圖。
使用直接法的場景要求：場景中要存在明暗變化（可以是漸變，不形成局部的圖像梯度）
直接法的分類：根據使用像素的數量，分為稀疏、稠密、半稠密
直接法是從光流演變而來，兩者非常相似，具有相同的假設條件。為了說明直接法，先來介紹光流。
1.光流（Optical FLow）
是一種描述像素隨時間在圖像之間運動的方法。
分類：1）稀疏光流——計算部分像素的運動，代表：Lucas-Kanade(LK)光流：通常被用來跟蹤角點的運動
灰度不變假設：同一個空間點的像素灰度值，在各個圖像中是固定不變的。（這是一個很強的假設，實際當中很可能不成立）
2）稠密光流——計算所有像素的運動
1.1 實踐：LK光流
演示如何使用OpenCV提供的光流法來跟蹤特征點:在實際應用中，LK光流的作用就是跟蹤特征點。與對每一幀提取特征點相比，使用LK光流只需要提取一次特征點，后續視頻幀只需要跟蹤就可以了，節約了許多特征提取時間。首先對第一幅圖像提取FAST角點，然后用LK光流跟蹤它們，並畫在圖中。
代碼如下：
1 #include <iostream> 2 #include <fstream> 3 #include <list> 4 #include <vector> 5 #include <chrono> 6 #include <opencv2/core/core.hpp> 7 #include <opencv2/highgui/highgui.hpp> 8 #include <opencv2/features2d/features2d.hpp> 9 #include <opencv2/video/tracking.hpp>//LK Flow 10 11 using namespace std; 12 13 int main(int argc,char** argv) 14 { 15 if(argc!=2) 16 { 17 cout<<"usage:useLK path_to_dataset"<<endl; 18 return 1; 19 } 20 21 string path_to_dataset=argv[1]; 22 //associate.txt:根據rgb和depth兩個文件的采集時間進行配對得到的 23 string associate_file=path_to_dataset+"/associate.txt"; 24 // cout<<associate_file<<endl; 25 ifstream fin(associate_file);//利用ifstream類的構造函數創建一個文件輸入流對象 26 if(!fin) 27 { 28 cerr<<"I cann't find associate.txt!"<<endl; 29 return 1; 30 } 31 32 string rgb_file,depth_file,time_rgb,time_depth; 33 list<cv::Point2f> keypoints;//因為要刪除跟蹤失敗的點，使用list 34 cv::Mat color,depth,last_color; 35 36 37 for(int index=0;index<100;++index) 38 { 39 fin>>time_rgb>>rgb_file>>time_depth>>depth_file;//從文件associate.txt中讀數據 40 // cout<<time_rgb<<" "<<rgb_file<<" "<<time_depth<<" "<<depth_file; 41 color=cv::imread(path_to_dataset+"/"+rgb_file); 42 depth=cv::imread(path_to_dataset+"/"+depth_file,-1); 43 // cv::imshow("color.png",color); 44 // cv::imshow("depth.png",depth); 45 if(index==0) 46 { 47 //對第一幀提取FAST特征點 48 //只對第一個圖像提取特征。然后對第一幅圖像提取的特征進行追蹤。 49 vector<cv::KeyPoint> kps; 50 cv::Ptr<cv::FastFeatureDetector> detector=cv::FastFeatureDetector::create(); 51 //檢測FAST角點位置 52 detector->detect(color,kps); 53 for(auto kp:kps) 54 keypoints.push_back(kp.pt); 55 last_color=color; 56 continue; 57 } 58 if(color.data==nullptr||depth.data==nullptr) 59 continue; 60 //對其他幀用LK跟蹤特征點 61 vector<cv::Point2f> next_keypoints; 62 vector<cv::Point2f> prev_keypoints; 63 for(auto kp:keypoints) 64 prev_keypoints.push_back(kp); 65 vector<unsigned char> status; 66 vector<float> error; 67 chrono::steady_clock::time_point t1=chrono::steady_clock::now(); 68 cv::calcOpticalFlowPyrLK(last_color,color,prev_keypoints,next_keypoints,status,error);//可得到新一幀中更新后的特征點位置 69 70 chrono::steady_clock::time_point t2=chrono::steady_clock::now(); 71 chrono::duration<double> time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1); 72 cout<<"LK Flow use time: "<<time_used.count()<<" seconds."<<endl; 73 //把跟丟的點刪掉 74 int i=0; 75 for(auto iter=keypoints.begin();iter!=keypoints.end();++i) 76 { 77 //status? 78 if(status[i]==0)//說明跟丟了 79 { 80 iter=keypoints.erase(iter); 81 continue; 82 } 83 *iter=next_keypoints[i]; 84 ++iter; 85 } 86 cout<<"tracked keypoints: "<<keypoints.size()<<endl; 87 if(keypoints.size()==0) 88 { 89 cout<<"all keypoints are lost."<<endl; 90 break; 91 } 92 //畫出keypoints 93 cv::Mat img_show=color.clone(); 94 for(auto kp:keypoints) 95 cv::circle(img_show,kp,10,cv::Scalar(0,240,0),1); 96 cv::imshow("corners",img_show); 97 cv::waitKey(0); 98 last_color=color; 99 } 100 return 0; 101 }
運行結果：
各幀特征點提取情況：
觀察特征點的跟蹤過程，我們發現1）位於物體角點處的特征更加穩定。2）邊緣處的特征會沿着邊緣“滑動”，主要是因為沿着邊緣移動時特征塊的內容基本不變，因此程序容易認為是同一個地方。3）既不在角點也不在邊緣的特征點則會頻繁跳動，位置非常不穩定。“金角銀邊草肚皮”：角點具有更好的辨識度，邊緣次之，區塊最少。
在本例中，LK跟蹤法避免了描述子的計算與匹配，但本身也需要一定的計算量。在具體的SLAM系統中，使用光流還是匹配描述子，最好自己做實驗測試。
LK光流跟蹤能夠直接得到特征點的對應關系，這個對應關系就像是描述子的匹配。
光流相對於描述子的優勢：在光流法中，，大多數時候只會碰到特征點跟丟的情況，而不太會遇到誤匹配。
缺點：匹配描述子的方法在相機運動較大時仍能成功，而光流法要求相機的運動必須是微小的。所以，光流的健壯性要差於描述子。
總結：光流法可加速基於特征點的視覺里程計算法，避免計算和匹配描述子的過程，但要求相機運動較慢（或采集頻率較高）。
2.直接法（Direct Method）
先介紹直接法的原理，然后使用g2o實現直接法。
推導直接法估計相機位姿的整個流程見P192-195。
根據一個已知位置的空間點P的來源，對直接法進行分類：
1）稀疏直接法——P來源於稀疏關鍵點。這種方法不必計算描述子，並且只使用數百個像素，因此速度最快，但只能計算稀疏的重構。
2）半稠密（Semi-Dense）直接法——P來源於部分像素：只使用帶有梯度的像素點，舍棄像素梯度不明顯的地方，因為如果像素梯度為零，整項雅可比矩陣就為零，不會對計算運動增量有任何貢獻（理論依據見P195式（8.16））。可重構一個半稠密結構。
3）稠密直接法——P為所有像素。稠密重構要計算所有像素，所以多數需要GPU的加速。對於像素梯度不明顯的點，在運動估計中的貢獻不大，在重構的時候也會難以估計位置。
總結：從1）->3）計算量逐漸增加。稀疏法可快速求解相機位姿，適應於實時性較高且計算資源有限的場合。稠密法可建立完整地圖。而具體應該使用哪種方法，應該視機器人的應用環境而定。
2.1實踐：RGB-D的直接法
2.1.1稀疏直接法（求解直接法=求解一個優化問圖：使用g2o優化庫）
先定義一種用於直接法位姿估計的邊，然后使用該邊構建圖優化問題並求解。
代碼：參考：https://www.cnblogs.com/newneul/p/8571653.html
1 #include <iostream> 2 #include <fstream> 3 #include <list> 4 #include <vector> 5 #include <chrono> 6 #include <ctime> 7 #include <climits> 8 9 #include <opencv2/core/core.hpp> 10 #include <opencv2/imgproc/imgproc.hpp> 11 #include <opencv2/highgui/highgui.hpp> 12 #include <opencv2/features2d/features2d.hpp> 13 //#include <Eigen/Core> 14 //#include <Eigen/Geometry>//Eigen::Isometry3d 15 16 17 #include <g2o/core/base_unary_edge.h> 18 #include <g2o/core/block_solver.h> 19 #include <g2o/core/optimization_algorithm_levenberg.h> 20 #include <g2o/solvers/dense/linear_solver_dense.h> 21 #include <g2o/core/robust_kernel.h> 22 #include <g2o/types/sba/types_six_dof_expmap.h> 23 24 using namespace std; 25 using namespace g2o; 26 using namespace cv; 27 //演示RGBD上的稀疏直接法 28 29 //一次測量的值，包括一個世界坐標系下三維點與一個灰度值 30 struct Measurement 31 { 32 Measurement(Eigen::Vector3d p,float g):pos_world(p),grayscale(g){} 33 Eigen::Vector3d pos_world; 34 float grayscale; 35 }; 36 37 38 inline Eigen::Vector3d project2Dto3D(int x,int y,int d,float fx,float fy,float cx,float cy,float scale) 39 { 40 float zz=float(d)/scale; 41 float xx=zz*(x-cx)/fx; 42 float yy=zz*(y-cy)/fy; 43 return Eigen::Vector3d(xx,yy,zz); 44 } 45 46 47 inline Eigen::Vector2d project3Dto2D ( float x, float y, float z, float fx, float fy, float cx, float cy ) 48 { 49 float u = fx*x/z+cx; 50 float v = fy*y/z+cy; 51 return Eigen::Vector2d ( u,v ); 52 } 53 54 //直接法估計位姿 55 //輸入：測量值（空間點的灰度），新的灰度圖，相機內參；輸出：相機位姿 56 //返回：true,成功;false,失敗 57 bool poseEstimationDirect(const vector<Measurement>& measurements,Mat* gray,Eigen::Matrix3f& intrinsics,Eigen::Isometry3d& Tcw); 58 59 60 //由於g2o中本身沒有計算光度誤差的邊，我們需要自己定義一種新的邊 61 class EdgeSE3ProjectDirect:public BaseUnaryEdge<1,double,VertexSE3Expmap> 62 { 63 public: 64 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 65 66 EdgeSE3ProjectDirect(){} 67 68 EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image ) 69 : x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image )//灰度圖像指針 70 {} 71 72 virtual void computeError() 73 { 74 const VertexSE3Expmap* v=static_cast<const VertexSE3Expmap*> (_vertices[0]); 75 Eigen::Vector3d x_local=v->estimate().map(x_world_); 76 float x=x_local[0]*fx_/x_local[2]+cx_;//u 77 float y=x_local[1]*fy_/x_local[2]+cy_;//v 78 //check x,y is in the image 79 //距離圖像四條邊4個像素大小的區域內作為有效投影區域，對於不在該范圍內的點誤差值設為0, 80 //為了防止計算的誤差太大，拉低內點對誤差的影響，導致估計的R,t嚴重偏離真值 81 if(x-4<0 || (x+4)>image_->cols || (y-4)<0 || (y+4)>image_->rows) 82 { 83 _error(0,0)=0.0; 84 this->setLevel(1); 85 } 86 else 87 { 88 _error(0,0)=getPixelValue(x,y)-_measurement;//經過在灰度圖中插值獲取得的像素值減去測量值 89 } 90 } 91 92 //plus in manifold 93 virtual void linearizeOplus() 94 { 95 if(level()==1) 96 { 97 _jacobianOplusXi=Eigen::Matrix<double,1,6>::Zero(); 98 return; 99 } 100 VertexSE3Expmap* vtx=static_cast<VertexSE3Expmap*> (_vertices[0]); 101 Eigen::Vector3d xyz_trans=vtx->estimate().map(x_world_); 102 103 double x=xyz_trans[0]; 104 double y=xyz_trans[1]; 105 double invz=1.0/xyz_trans[2]; 106 double invz_2=invz*invz; 107 108 float u=x*fx_*invz+cx_; 109 float v=y*fy_*invz+cy_; 110 111 //jacobin from se3 to u,v 112 //NOTE that in g2o the Lie alfebra is 113 Eigen::Matrix<double,2,6> jacobian_uv_ksai; 114 115 jacobian_uv_ksai(0,0)=-x*y*invz_2*fx_; 116 jacobian_uv_ksai(0,1)=(1+(x*x*invz_2))*fx_; 117 jacobian_uv_ksai(0,2)=-y*fx_*invz; 118 jacobian_uv_ksai(0,3)=invz*fx_; 119 jacobian_uv_ksai(0,4)=0; 120 jacobian_uv_ksai(0,5)=-x*fx_*invz_2; 121 122 jacobian_uv_ksai(1,0)=-(1+(y*y*invz_2))*fy_; 123 jacobian_uv_ksai(1,1)=x*y*invz_2*fy_; 124 jacobian_uv_ksai(1,2)=x*invz*fy_; 125 jacobian_uv_ksai(1,3)=0; 126 jacobian_uv_ksai(1,4)=invz*fy_; 127 jacobian_uv_ksai(1,5)=-y*invz_2*fy_; 128 129 Eigen::Matrix<double,1,2> jacobian_pixel_uv; 130 131 //書上I2對像素坐標系的偏導數 這里很有可能 計算出來的梯度為0 因為FAST角點的梯度沒有限制 132 //這也是半稠密法主要改進的地方 就是選關鍵點的時候 選擇梯度大的點 因此這里的梯度就不可能為0了 133 jacobian_pixel_uv(0,0)=(getPixelValue(u+1,v)-getPixelValue(u-1,v))/2; 134 jacobian_pixel_uv(0,1)=(getPixelValue(u,v+1)-getPixelValue(u,v-1))/2; 135 136 _jacobianOplusXi=jacobian_pixel_uv*jacobian_uv_ksai; 137 } 138 139 //dummy read and write functions because we don't care 140 virtual bool read(std::istream& in){} 141 virtual bool write(std::ostream& out) const{} 142 143 protected: 144 //get a gray scale value from reference image(billinear interpolated) 145 //下面的方式：針對但通道灰度圖 146 inline float getPixelValue(float x,float y)//通過雙線性插值獲取浮點坐標對應的插值后的像素值 147 { 148 uchar* data=& image_->data[int(y)*image_->step+int(x)]; 149 float xx=x-floor(x); 150 float yy=y-floor(y); 151 return float(//公式f(i+u,j+v)=(1-u)(1-v)f(i,j)+u(1-v)f(i+1,j)+(1-u)vf(i,j+1)+uvf(i+1,j+1) 152 //這里的xx就是u，yy就是v 153 (1-xx)*(1-yy)*data[0]+ 154 xx*(1-yy)*data[1]+ 155 (1-xx)*yy*data[image_->step]+ 156 xx*yy*data[image_->step+1] 157 ); 158 } 159 public: 160 Eigen::Vector3d x_world_;//3D point in world frame 161 float cx_=0,cy_=0,fx_=0,fy_=0;//Camera insics 162 Mat* image_=nullptr;//reference image 163 }; 164 165 int main(int argc,char** argv) 166 { 167 if(argc!=2) 168 { 169 cout<<"usage:direct_sparse path_to_dataset"<<endl; 170 return 1; 171 } 172 srand((unsigned int) time(0));//根據時間生成一個隨機數 173 //srand目的就是讓得到的序列看上去更貼近隨機的概念 174 string path_to_dataset=argv[1]; 175 string associate_file=path_to_dataset+"/associate.txt"; 176 177 ifstream fin(associate_file); 178 179 string rgb_file,depth_file,time_rgb,time_depth; 180 Mat color,depth,gray; 181 vector<Measurement> measurements;//Measurement類 存儲世界坐標點（以第一幀為參考的FAST關鍵點） 182 //和對應的灰度圖像（由color->gray）的灰度值 183 184 //相機內參 185 float cx=325.5; 186 float cy=253.5; 187 float fx=518.0; 188 float fy=519.0; 189 float depth_scale=1000.0; 190 Eigen::Matrix3f K; 191 K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f; 192 // cout<<K<<endl; 193 194 Eigen::Isometry3d Tcw=Eigen::Isometry3d::Identity(); 195 196 Mat prev_color; 197 //我們以第一幅圖像為參考，對后續圖像和參考圖像做直接法 198 //每一副圖像都會與第一幀圖像做直接法計算第一幀到當前幀的R,t。 199 //但是經過更多幀后，關鍵點數量會減少 200 //所以實際應用時，應當規定關鍵點的數量少於多少時就該重新設定參考系，再次利用直接法，但是會有累計的誤差需要解決 201 for(int index=0;index<10;++index) 202 { 203 cout<<"******************* loop "<<index<<" ************"<<endl; 204 fin>>time_rgb>>rgb_file>>time_depth>>depth_file; 205 // cout<<time_rgb<<" "<<rgb_file<<" "<<time_depth<<" "<<depth_file<<endl; 206 color=imread(path_to_dataset+"/"+rgb_file); 207 depth=imread(path_to_dataset+"/"+depth_file,-1);//-1 按原圖像的方式存儲 detph:16位存儲 208 if(color.data==nullptr || depth.data==nullptr) 209 continue; 210 //轉換后的灰度圖為g2o優化需要的邊提供灰度值 211 cvtColor(color,gray,COLOR_BGR2GRAY);//cv::cvtColor()用於將圖像從一個顏色空間轉換到另一個顏色空間 212 //在轉換的過程中能夠保證數據的類型不變 213 //即轉換后的圖像的數據類型和位深與源圖像一致 214 215 //第一幀為世界坐標系，計算FAST關鍵點，為之后與當前幀用直接法計算R,t做准備 216 if(index==0)//以第一幀為參考系，計算關鍵點后存儲測量值（關鍵點對應的灰度值），以此為基准跟蹤后面的圖像，計算位姿 217 { 218 imshow("color",color); 219 imshow("depth",depth); 220 //對第一幀提取FAST角點,為什么要這樣做？？？？ 221 vector<KeyPoint> keypoints; 222 //OpenCV提供FeatureDetector實現特征檢測及匹配 223 Ptr<FastFeatureDetector> detector=FastFeatureDetector::create(); 224 detector->detect(color,keypoints); 225 226 Mat outimg; 227 drawKeypoints(color,keypoints,outimg,Scalar::all(-1),DrawMatchesFlags::DEFAULT); 228 imshow("corner",outimg); 229 230 //對於2D關鍵點獲取3D信息，並去掉范圍外的點，存儲符合要求的關鍵點的深度值和3D信息 231 //對所有關鍵點挑選出符合要求且有深度值的存儲到vector<Measurement> measurements為g2o邊提供灰度測量值和空間點坐標 232 for(auto kp:keypoints) 233 { 234 //去掉鄰近邊緣處的點:考慮是否能用圖畫出來 235 //在離圖像四條邊20個像素構成的內矩陣范圍內是符合要求的關鍵點 236 // cout<<"kp.pt.x = "<<kp.pt.x<<endl<<"kp.pt.y = "<<kp.pt.y<<endl 237 // <<"color.cols = "<<color.rows<<endl; 238 if(kp.pt.x<20 || kp.pt.y<20 || (kp.pt.x+20)>color.cols || (kp.pt.y+20)>color.rows) 239 continue; 240 //depth.ptr (cvRound(kp.pt.y))表示獲取行指針 241 //cvRound(kp.pt.y)表示返回跟參數值最接近的整數值 242 //因為像素量化以后是整數，而kp.pt.y存儲方式是float,所以強制轉換一下即可 243 ushort d=depth.ptr<ushort> (cvRound(kp.pt.y))[cvRound(kp.pt.x)]; 244 if(d==0) 245 continue; 246 Eigen::Vector3d p3d=project2Dto3D(kp.pt.x,kp.pt.y,d,fx,fy,cx,cy,depth_scale);//3D相機坐標系（第一幀，也是世界幀） 247 float grayscale=float (gray.ptr<uchar>(cvRound(kp.pt.y))[cvRound(kp.pt.x)]); 248 measurements.push_back(Measurement(p3d,grayscale)); 249 } 250 prev_color=color.clone(); 251 continue; 252 } 253 //使用直接法計算相機運動 254 //從第二幀開始計算相機位姿g2o優化 255 chrono::steady_clock::time_point t1=chrono::steady_clock::now(); 256 // 257 poseEstimationDirect(measurements,&gray,K,Tcw); 258 chrono::steady_clock::time_point t2=chrono::steady_clock::now(); 259 chrono::duration<double> time_used=chrono::duration_cast<chrono::duration<double> >(t2-t1); 260 cout<<"direct method costs time: "<<time_used.count()<<" seconds."<<endl; 261 cout<<"Tcw = "<<Tcw.matrix()<<endl; 262 263 264 // plot the feature points 265 cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 );//目的是為了之后對比前后兩幀圖像的關鍵點數量 266 //所以建立一個可以存儲pre_co和color大小的矩陣 267 //Rect參數表示坐標0,0到cols,rows那么大的矩陣 268 prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) );//0列，0行->cols列,rows行，大小 269 //實際上就是把第一幀的圖像拷貝到img_show中 270 //因為我們針對每一幀圖像都會把第一幀圖像拷貝到這里，所以這里實際上執行一次即可 271 color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) );//0列,rows行->cols列 rows行 272 //在measurement容器中，隨機挑選出符合要求的測量值，在img_show矩陣中對應部分進行標記（因為img_show上半部分是第一幀圖像，下半部分是當前圖像） 273 for ( Measurement m:measurements ) 274 { 275 if ( rand() > RAND_MAX/5 ) 276 continue; 277 Eigen::Vector3d p = m.pos_world;//將空間點轉換到下一幀相機坐標系下 278 Eigen::Vector2d pixel_prev = project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy ); 279 Eigen::Vector3d p2 = Tcw*m.pos_world; 280 Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy ); 281 282 //對於超出下一幀圖像像素坐標軸范圍的點舍棄不畫 283 if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows ) 284 continue; 285 286 //隨機獲取bgr顏色，在cv::circle中，為關鍵點用不同的顏色圓來畫出 287 float b = 255*float ( rand() ) /RAND_MAX; 288 float g = 255*float ( rand() ) /RAND_MAX; 289 float r = 255*float ( rand() ) /RAND_MAX; 290 291 //在img_show包含兩幀圖像上，以關鍵點為圓心畫圓，半徑為8個像素，顏色為bgr隨機組合，2表示外輪廓線寬度為2，如果為負數表示填充圓 292 //pixel_prev都是世界坐標系下的坐標（以第一幀為參考系）和當前幀的對比，可以看出關鍵點的數量會逐漸減少 293 cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 8, cv::Scalar ( b,g,r ), 2 ); 294 cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 8, cv::Scalar ( b,g,r ), 2 ); 295 //連接后兩幀匹配好的點 296 cv::line ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), cv::Scalar ( b,g,r ), 1 ); 297 } 298 cv::imshow ( "result", img_show ); 299 waitKey(0); 300 } 301 302 return 0; 303 } 304 305 bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw ) 306 { 307 // 初始化g2o 308 typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock; // 求解的向量是6＊1的 309 DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > (); 310 DirectBlock* solver_ptr = new DirectBlock ( unique_ptr<DirectBlock::LinearSolverType>(linearSolver) ); 311 // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N 312 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( unique_ptr<DirectBlock>(solver_ptr) ); // L-M 313 g2o::SparseOptimizer optimizer; 314 optimizer.setAlgorithm ( solver ); 315 optimizer.setVerbose( true ); 316 317 g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); 318 pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) ); 319 pose->setId ( 0 ); 320 optimizer.addVertex ( pose ); 321 322 // 添加邊 323 int id=1; 324 for ( Measurement m: measurements ) 325 { 326 EdgeSE3ProjectDirect* edge = new EdgeSE3ProjectDirect ( 327 m.pos_world, 328 K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray 329 ); 330 edge->setVertex ( 0, pose ); 331 edge->setMeasurement ( m.grayscale ); 332 edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() ); 333 edge->setId ( id++ ); 334 optimizer.addEdge ( edge ); 335 } 336 cout<<"edges in graph: "<<optimizer.edges().size() <<endl; 337 optimizer.initializeOptimization(); 338 optimizer.optimize ( 30 ); 339 Tcw = pose->estimate(); 340 }
運行結果：我們貼出一部分試驗結果圖：參考幀與后1至3幀對比
2.1.2 半稠密直接法
對參考幀，先提取梯度較明顯的像素，然后用直接法，以這些像素為圖優化邊來估計相機運動。與2.1.1 稀疏直接法相比，我們把先前的稀疏特征點改成了帶有明顯梯度的像素。於是在圖優化中增加許多的邊。這些邊都會參與估計相機位姿的優化問題，利用大量的像素而不單單是稀疏的特征點。
代碼如下：
1 #include <iostream> 2 #include <fstream> 3 #include <list> 4 #include <vector> 5 #include <chrono> 6 #include <ctime> 7 #include <climits> 8 9 #include <opencv2/core/core.hpp> 10 #include <opencv2/imgproc/imgproc.hpp> 11 #include <opencv2/highgui/highgui.hpp> 12 #include <opencv2/features2d/features2d.hpp> 13 14 #include <g2o/core/base_unary_edge.h> 15 #include <g2o/core/block_solver.h> 16 #include <g2o/core/optimization_algorithm_levenberg.h> 17 #include <g2o/solvers/dense/linear_solver_dense.h> 18 #include <g2o/core/robust_kernel.h> 19 #include <g2o/types/sba/types_six_dof_expmap.h> 20 21 using namespace std; 22 using namespace g2o; 23 24 /******************************************** 25 * 本節演示了RGBD上的半稠密直接法 26 ********************************************/ 27 28 // 一次測量的值，包括一個世界坐標系下三維點與一個灰度值 29 struct Measurement 30 { 31 Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {} 32 Eigen::Vector3d pos_world; 33 float grayscale; 34 }; 35 36 inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale ) 37 { 38 float zz = float ( d ) /scale; 39 float xx = zz* ( x-cx ) /fx; 40 float yy = zz* ( y-cy ) /fy; 41 return Eigen::Vector3d ( xx, yy, zz ); 42 } 43 44 inline Eigen::Vector2d project3Dto2D ( float x, float y, float z, float fx, float fy, float cx, float cy ) 45 { 46 float u = fx*x/z+cx; 47 float v = fy*y/z+cy; 48 return Eigen::Vector2d ( u,v ); 49 } 50 51 // 直接法估計位姿 52 // 輸入：測量值（空間點的灰度），新的灰度圖，相機內參； 輸出：相機位姿 53 // 返回：true為成功，false失敗 54 bool poseEstimationDirect ( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw ); 55 56 57 // project a 3d point into an image plane, the error is photometric error 58 // an unary edge with one vertex SE3Expmap (the pose of camera) 59 class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap> 60 { 61 public: 62 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 63 64 EdgeSE3ProjectDirect() {} 65 66 EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image ) 67 : x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image ) 68 {} 69 70 virtual void computeError() 71 { 72 const VertexSE3Expmap* v =static_cast<const VertexSE3Expmap*> ( _vertices[0] ); 73 Eigen::Vector3d x_local = v->estimate().map ( x_world_ ); 74 float x = x_local[0]*fx_/x_local[2] + cx_; 75 float y = x_local[1]*fy_/x_local[2] + cy_; 76 // check x,y is in the image 77 if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows ) 78 { 79 _error ( 0,0 ) = 0.0; 80 this->setLevel ( 1 ); 81 } 82 else 83 { 84 _error ( 0,0 ) = getPixelValue ( x,y ) - _measurement; 85 } 86 } 87 88 // plus in manifold 89 virtual void linearizeOplus( ) 90 { 91 if ( level() == 1 ) 92 { 93 _jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero(); 94 return; 95 } 96 VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] ); 97 Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ ); // q in book 98 99 double x = xyz_trans[0]; 100 double y = xyz_trans[1]; 101 double invz = 1.0/xyz_trans[2]; 102 double invz_2 = invz*invz; 103 104 float u = x*fx_*invz + cx_; 105 float v = y*fy_*invz + cy_; 106 107 // jacobian from se3 to u,v 108 // NOTE that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation 109 Eigen::Matrix<double, 2, 6> jacobian_uv_ksai; 110 111 jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_; 112 jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_; 113 jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_; 114 jacobian_uv_ksai ( 0,3 ) = invz *fx_; 115 jacobian_uv_ksai ( 0,4 ) = 0; 116 jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_; 117 118 jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_; 119 jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_; 120 jacobian_uv_ksai ( 1,2 ) = x*invz *fy_; 121 jacobian_uv_ksai ( 1,3 ) = 0; 122 jacobian_uv_ksai ( 1,4 ) = invz *fy_; 123 jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_; 124 125 Eigen::Matrix<double, 1, 2> jacobian_pixel_uv; 126 127 jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2; 128 jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2; 129 130 _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai; 131 } 132 133 // dummy read and write functions because we don't care... 134 virtual bool read ( std::istream& in ) {} 135 virtual bool write ( std::ostream& out ) const {} 136 137 protected: 138 // get a gray scale value from reference image (bilinear interpolated) 139 inline float getPixelValue ( float x, float y ) 140 { 141 uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ]; 142 float xx = x - floor ( x ); 143 float yy = y - floor ( y ); 144 return float ( 145 ( 1-xx ) * ( 1-yy ) * data[0] + 146 xx* ( 1-yy ) * data[1] + 147 ( 1-xx ) *yy*data[ image_->step ] + 148 xx*yy*data[image_->step+1] 149 ); 150 } 151 public: 152 Eigen::Vector3d x_world_; // 3D point in world frame 153 float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics 154 cv::Mat* image_=nullptr; // reference image 155 }; 156 157 int main ( int argc, char** argv ) 158 { 159 if ( argc != 2 ) 160 { 161 cout<<"usage: useLK path_to_dataset"<<endl; 162 return 1; 163 } 164 srand ( ( unsigned int ) time ( 0 ) ); 165 string path_to_dataset = argv[1]; 166 string associate_file = path_to_dataset + "/associate.txt"; 167 168 ifstream fin ( associate_file ); 169 170 string rgb_file, depth_file, time_rgb, time_depth; 171 cv::Mat color, depth, gray; 172 vector<Measurement> measurements; 173 // 相機內參 174 float cx = 325.5; 175 float cy = 253.5; 176 float fx = 518.0; 177 float fy = 519.0; 178 float depth_scale = 1000.0; 179 Eigen::Matrix3f K; 180 K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f; 181 182 Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity(); 183 184 cv::Mat prev_color; 185 // 我們以第一個圖像為參考，對后續圖像和參考圖像做直接法 186 for ( int index=0; index<10; index++ ) 187 { 188 cout<<"*********** loop "<<index<<" ************"<<endl; 189 fin>>time_rgb>>rgb_file>>time_depth>>depth_file; 190 color = cv::imread ( path_to_dataset+"/"+rgb_file ); 191 depth = cv::imread ( path_to_dataset+"/"+depth_file, -1 ); 192 if ( color.data==nullptr || depth.data==nullptr ) 193 continue; 194 cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY ); 195 if ( index ==0 ) 196 { 197 // select the pixels with high gradiants 198 for ( int x=10; x<gray.cols-10; x++ ) 199 for ( int y=10; y<gray.rows-10; y++ ) 200 { 201 Eigen::Vector2d delta ( 202 gray.ptr<uchar>(y)[x+1] - gray.ptr<uchar>(y)[x-1], 203 gray.ptr<uchar>(y+1)[x] - gray.ptr<uchar>(y-1)[x] 204 ); 205 if ( delta.norm() < 50 ) 206 continue; 207 ushort d = depth.ptr<ushort> (y)[x]; 208 if ( d==0 ) 209 continue; 210 Eigen::Vector3d p3d = project2Dto3D ( x, y, d, fx, fy, cx, cy, depth_scale ); 211 float grayscale = float ( gray.ptr<uchar> (y) [x] ); 212 measurements.push_back ( Measurement ( p3d, grayscale ) ); 213 } 214 prev_color = color.clone(); 215 cout<<"add total "<<measurements.size()<<" measurements."<<endl; 216 continue; 217 } 218 // 使用直接法計算相機運動 219 chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); 220 poseEstimationDirect ( measurements, &gray, K, Tcw ); 221 chrono::steady_clock::time_point t2 = chrono::steady_clock::now(); 222 chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 ); 223 cout<<"direct method costs time: "<<time_used.count() <<" seconds."<<endl; 224 cout<<"Tcw="<<Tcw.matrix() <<endl; 225 226 // plot the feature points 227 cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 ); 228 prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) ); 229 color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) ); 230 for ( Measurement m:measurements ) 231 { 232 if ( rand() > RAND_MAX/5 ) 233 continue; 234 Eigen::Vector3d p = m.pos_world; 235 Eigen::Vector2d pixel_prev = project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy ); 236 Eigen::Vector3d p2 = Tcw*m.pos_world; 237 Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy ); 238 if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows ) 239 continue; 240 241 float b = 0; 242 float g = 250; 243 float r = 0; 244 img_show.ptr<uchar>( pixel_prev(1,0) )[int(pixel_prev(0,0))*3] = b; 245 img_show.ptr<uchar>( pixel_prev(1,0) )[int(pixel_prev(0,0))*3+1] = g; 246 img_show.ptr<uchar>( pixel_prev(1,0) )[int(pixel_prev(0,0))*3+2] = r; 247 248 img_show.ptr<uchar>( pixel_now(1,0)+color.rows )[int(pixel_now(0,0))*3] = b; 249 img_show.ptr<uchar>( pixel_now(1,0)+color.rows )[int(pixel_now(0,0))*3+1] = g; 250 img_show.ptr<uchar>( pixel_now(1,0)+color.rows )[int(pixel_now(0,0))*3+2] = r; 251 cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 4, cv::Scalar ( b,g,r ), 2 ); 252 cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 4, cv::Scalar ( b,g,r ), 2 ); 253 } 254 cv::imshow ( "result", img_show ); 255 cv::waitKey ( 0 ); 256 257 } 258 return 0; 259 } 260 261 bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw ) 262 { 263 // 初始化g2o 264 typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock; // 求解的向量是6＊1的 265 DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > (); 266 DirectBlock* solver_ptr = new DirectBlock (unique_ptr<DirectBlock::LinearSolverType> (linearSolver) ); 267 // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N 268 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg (unique_ptr<DirectBlock> (solver_ptr) ); // L-M 269 g2o::SparseOptimizer optimizer; 270 optimizer.setAlgorithm ( solver ); 271 optimizer.setVerbose( true ); 272 273 g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); 274 pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) ); 275 pose->setId ( 0 ); 276 optimizer.addVertex ( pose ); 277 278 // 添加邊 279 int id=1; 280 for ( Measurement m: measurements ) 281 { 282 EdgeSE3ProjectDirect* edge = new EdgeSE3ProjectDirect ( 283 m.pos_world, 284 K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray 285 ); 286 edge->setVertex ( 0, pose ); 287 edge->setMeasurement ( m.grayscale ); 288 edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() ); 289 edge->setId ( id++ ); 290 optimizer.addEdge ( edge ); 291 } 292 cout<<"edges in graph: "<<optimizer.edges().size() <<endl; 293 optimizer.initializeOptimization(); 294 optimizer.optimize ( 30 ); 295 Tcw = pose->estimate(); 296 }
分析：1）參與估計的像素像是固定在空間中一樣。當相機旋轉時，它們的位置似乎沒有發生變化。這代表了我們估計的相機運動是正確的。
2）相比於特征點法，直接法完全依靠優化來求解相機位姿。
直接法優缺點總結
優點：1）可省去計算特征點、描述子的時間。
2）只要求有像素梯度即可，不需要特征點。
因此，直接法可以在特征點缺失的場合下使用，比較極端的例子是只有漸變的一幅圖像，它可能無法提取角點類特征，但可以用直接法估計它的運動。
3）可以構建半稠密乃至稠密的地圖，這是特征點法無法做到。
缺點：1）非凸性。直接法完全依靠梯度搜索，降低目標函數來計算相機位姿。其目標函數中需要取像素點的灰度值，而圖像是強烈非凸的函數，這使得優化算法容易進入極小，只在運動很小時直接法才能成功。
2）單個像素沒有區分度。
3）灰度值不變是很強的假設。