視覺SLAM十四講課後習題—ch8 《視覺SLAM十四講》筆記（ch8）
阿新 • • 發佈：2018-12-09
1.除了LK光流之外，還有哪些光流方法？它們各有什麼特點？
參考：https://blog.csdn.net/on2way/article/details/48953975
1）LK(Lucas-Kanada)光流： 一種經典的方法，基於區域性特徵計算光流
2）Horn-Schunck光流方法：全域性性質下計算光流的方法，該方法運算較慢
3）Buxton方法：一種基於邊緣運動的影象序列模型的方法
4）Black-Jepson方法
2.在本節程式的求影象梯度過程中，我們簡單地求了u+1和u-1的灰度之差處以2，作為u方向上的梯度值。這種做法有什麼缺點？提示：對於距離較近的特徵，變化應該較快，而距離較遠的特徵在影象中變化較慢，求梯度時能否利用此資訊？
本節程式中求影象梯度的過程：
jacobian_pixel_uv(0,0)=(getPixelValue(u+1,v)-getPixelValue(u-1,v))/2;
jacobian_pixel_uv(0,1)=(getPixelValue(u,v+1)-getPixelValue(u,v-1))/2；
顯然，當灰度值變化很快的時候，u方向上的梯度值是不能等於u+1處的梯度值和u-1處的梯度值之差除以2的。
3.在稀疏直接法中，假設單個畫素周圍小塊的光度也不變，是否可以提高演算法的健壯性，請程式設計實現。
分析：直接法是基於灰度不變假設，即同一個空間點的畫素灰度值在各個影象中是固定不變的。如果現在假設單個畫素周圍小塊的光度也不變，我們就可以用單個畫素周圍小塊的平均灰度值來代替單個畫素的灰度值。至於是否能提高健壯性，還需實驗和評估。
類似的思想見ch13講中的塊匹配
在稀疏直接法實踐（見《視覺SLAM十四講》筆記（ch8）2.1實踐）的基礎上更改程式：
參考：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> 89 #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 int patchRadius=1; 25 //獲取小塊平均灰度值 26 // gray:灰度矩陣 x,y表示以(x,y)為中心 27 //計算的小塊的平均灰度 28 float getPatchAverageGray(const cv::Mat &gray ,float u, float v); 29 30 31 struct Measurement 32 { 33 Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {} 34 Eigen::Vector3d pos_world; 35 float grayscale; 36 }; 37 38 //轉換成相機座標系下座標 39 inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale ) 40 { 41 float zz = float ( d ) /scale; 42 float xx = zz* ( x-cx ) /fx; 43 float yy = zz* ( y-cy ) /fy; 44 return Eigen::Vector3d ( xx, yy, zz ); 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 //輸出：相機位姿 57 void poseEstimationDirect ( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw ); 58 59 // project a 3d point into an image plane, the error is photometric error 60 // an unary edge with one vertex SE3Expmap (the pose of camera) 61 //誤差值維度 誤差型別 頂點型別 62 class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap> 63 { 64 public: 65 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 66 67 EdgeSE3ProjectDirect() = default; //代替下面的方式 預設產生合成的建構函式 68 //EdgeSE3ProjectDirect(){} 69 EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image ) 70 : x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image ) //灰度影象指標 71 {} 72 73 virtual void computeError() 74 { 75 const VertexSE3Expmap* v =static_cast<const VertexSE3Expmap*> ( _vertices[0] ); 76 Eigen::Vector3d x_local = v->estimate().map ( x_world_ ); 77 float x = x_local[0]*fx_/x_local[2] + cx_; //世界座標轉換到當期幀畫素座標 78 float y = x_local[1]*fy_/x_local[2] + cy_; 79 // check x,y is in the image 80 //距離影象四條邊4個畫素大小的區域內作為有效投影區域 對於不在該範圍內的點誤差值設為0 為了防止計算的誤差太大 拉低內點對誤差的影響 導致估計的RT嚴重偏離真值 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 float sumValue = 0.0; 89 for(int i = x-1; i<=x+1 ; ++i) 90 for (int j = y-1; j <= y+1 ; ++j) { 91 sumValue += getPixelValue(i,j); 92 } 93 //sumValue /=( (2*PATCH_RADIUS +1)*(2*PATCH_RADIUS+1) ); //求得元素周圍小塊的平均灰度值 94 sumValue/=9; 95 _error (0,0) = sumValue - _measurement; 96 } 97 } 98 99 // plus in manifold 100 //提供誤差關於位姿的雅克比矩陣 書上8.16式子 只不過負號去掉了 因為用的是當前幀灰度值 - 世界座標下的測量值 101 virtual void linearizeOplus( )override 102 { 103 if ( level() == 1 ) 104 { 105 _jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero(); 106 return; 107 } 108 VertexSE3Expmap* vtx = dynamic_cast<VertexSE3Expmap*> ( _vertices[0] ); 109 Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ ); // q in book 轉換到第二幀座標系下 110 111 double x = xyz_trans[0]; 112 double y = xyz_trans[1]; 113 double invz = 1.0/xyz_trans[2]; 114 double invz_2 = invz*invz; 115 116 float u = x*fx_*invz + cx_;//投影到第二幀畫素座標系 117 float v = y*fy_*invz + cy_; 118 119 // jacobian from se3 to u,v 120 // NOTE that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation 121 Eigen::Matrix<double, 2, 6> jacobian_uv_ksai; 122 123 //書上8.15式子 124 jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_; 125 jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_; 126 jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_; 127 jacobian_uv_ksai ( 0,3 ) = invz *fx_; 128 jacobian_uv_ksai ( 0,4 ) = 0; 129 jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_; 130 131 jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_; 132 jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_; 133 jacobian_uv_ksai ( 1,2 ) = x*invz *fy_; 134 jacobian_uv_ksai ( 1,3 ) = 0; 135 jacobian_uv_ksai ( 1,4 ) = invz *fy_; 136 jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_; 137 138 Eigen::Matrix<double, 1, 2> jacobian_pixel_uv; 139 140 //書上I2對畫素座標系的偏導數 這裡很有可能 計算出來的梯度為0 因為FAST角點的梯度沒有限制 141 //這也是半稠密法主要改進的地方 就是選關鍵點的時候 選擇梯度大的點 因此這裡的梯度就不可能為0了 142 jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2; 143 jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2; 144 145 _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;//書上8.16式子 146 } 147 148 // dummy read and write functions because we don't care... 149 virtual bool read ( std::istream& in ) {} 150 virtual bool write ( std::ostream& out ) const {} 151 152 protected: 153 // get a gray scale value from reference image (bilinear interpolated) 154 //下面的方式 針對單通道的灰度圖 155 inline float getPixelValue ( float x, float y )//通過雙線性插值獲取浮點座標對應的插值後的畫素值 156 { 157 uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];//step表示影象矩陣一行的所有位元組（包括所有通道的總和），data表示儲存影象的開始指標 158 float xx = x - floor ( x ); //取整函式 159 float yy = y - floor ( y ); 160 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) 161 //這裡的xx 就是u yy就是v 162 ( 1-xx ) * ( 1-yy ) * data[0] + 163 xx* ( 1-yy ) * data[1] + 164 ( 1-xx ) *yy*data[ image_->step ] + //I(i+1,j) //這裡相當於畫素的週期是image_->step，即每一行儲存畫素的個數為image_->step 165 xx*yy*data[image_->step+1] //I(i+1,j+1) //data[image_->step]是I(i,j)對應的下一行畫素為I(i+1,j) 166 ); 167 } 168 public: 169 Eigen::Vector3d x_world_; // 3D point in world frame 170 float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics 171 cv::Mat* image_=nullptr; // reference image 172 }; 173 174 int main ( int argc, char** argv ) 175 { 176 if ( argc != 2 ) 177 { 178 cout<<"usage: useLK path_to_dataset"<<endl; 179 return 1; 180 } 181 srand ( ( unsigned int ) time ( 0 ) ); 182 string path_to_dataset = argv[1]; 183 string associate_file = path_to_dataset + "/associate.txt"; 184 185 ifstream fin ( associate_file ); 186 187 string rgb_file, depth_file, time_rgb, time_depth; 188 cv::Mat color, depth, gray; 189 vector<Measurement> measurements;//Measurement類 儲存世界座標點（以第一幀為參考的FAST關鍵點） 和 對應的灰度影象（由color->gray）的灰度值 190 // 相機內參 191 float cx = 325.5; 192 float cy = 253.5; 193 float fx = 518.0; 194 float fy = 519.0; 195 float depth_scale = 1000.0; 196 Eigen::Matrix3f K; 197 K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f; 198 199 Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();//三維變換矩陣T 4X4 初始時刻是單位R矩陣+0平移向量 200 201 cv::Mat prev_color; 202 // 我們以第一個影象為參考，對後續影象和參考影象做直接法 ，每一副影象 都會與第一幀影象做直接法計算第一幀到當前幀的RT 但是經過更多的幀後 關鍵點的數量會減少， 203 //所以實際應用時 應當規定關鍵點的數量少於多少 就該從新設定參考系，再次利用直接法 ，但是會累計的誤差需要解決？？？？ 204 for ( int index=0; index<10; index++ )//總共10幀 205 { 206 cout<<"*********** loop "<<index<<" ************"<<endl; 207 fin>>time_rgb>>rgb_file>>time_depth>>depth_file; 208 color = cv::imread ( path_to_dataset+"/"+rgb_file ); 209 depth = cv::imread ( path_to_dataset+"/"+depth_file, -1 );//-1 按原影象的方式儲存 detph 16位儲存 210 if ( color.data==nullptr || depth.data==nullptr ) 211 continue; 212 //轉換後的灰度圖為g2o優化需要的邊提供灰度值 213 cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY ); //將顏色圖3通道 轉換為灰度圖單通道 8位無符號 對應邊類的雙線性插值計算放法以單通道計算的 214 215 //第一幀為世界座標系 計算FAST關鍵點 為之後與當前幀用直接法計算RT做準備 216 if ( index ==0 )//以第一幀為參考系 計算關鍵點後儲存測量值（關鍵點對應的灰度值） 以此為基準跟蹤後面的影象 計算位姿 217 { 218 // 對第一幀提取FAST特徵點 219 vector<cv::KeyPoint> keypoints; 220 cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create(); 221 detector->detect ( color, keypoints ); 222 //對於2D關鍵點獲取 3D資訊 並去掉範圍外的點 儲存符合要求的關鍵點的深度值和3D資訊 223 //對所有關鍵點挑選出符合要求且有深度值的 儲存到vector<Measurement> measurements中 為g2o邊提供灰度測量值和空間點座標 224 for ( auto kp:keypoints ) 225 { 226 // 去掉鄰近邊緣處的點 在離影象四條邊20個畫素構成的內矩陣範圍內是符合要求的關鍵點 227 if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows ) 228 continue; 229 //depth.ptr<ushort>( kp.pt.y)獲取行指標 cvRound(kp.pt,y) 表示返回跟引數值最接近的整數值 因為畫素量化後是整數，而kp.pt.y儲存方式是float，所以強制轉換一下即可 230 ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];//16位深度圖 231 if ( d==0 ) 232 continue; 233 Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale ); //3D相機座標系（第一幀 也是世界幀） 234 //計算小塊平均灰度值作為對應單一畫素的測量值 增加演算法健壯性 235 float grayscale = getPatchAverageGray( gray , kp.pt.x , kp.pt.y); 236 237 238 measurements.push_back ( Measurement ( p3d, grayscale ) ); 239 } 240 prev_color = color.clone(); //深拷貝color影象 241 continue; 242 } 243 // 使用直接法計算相機運動 244 //從第二幀開始計算相機位姿g2o優化 245 chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); 246 //優化過程中要提供灰度影象 邊裡面計算誤差函式需要 為getPixelValue()該函式提供灰度值查詢 247 poseEstimationDirect ( measurements, &gray, K, Tcw );//Tcw為世界座標到下一幀座標的累計值 最後Tcw的結果是從世界座標 到當前幀下的轉換 248 chrono::steady_clock::time_point t2 = chrono::steady_clock::now(); 249 chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 ); 250 cout<<"direct method costs time: "<<time_used.count() <<" seconds."<<endl; 251 cout<<"Tcw="<<Tcw.matrix() <<endl; 252 253 // plot the feature points 254 cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 );//目的是為了之後對比前後兩幀影象的關鍵點數量 所以建立一個可以儲存pre_color 和color 大小的矩陣 255 //Rect(引數)表示座標0,0 到cols,rows 那麼大的矩形 256 //img_show.operator(const Rect &roi 引數:表示這個矩陣的某個興趣區域) 257 //img_show.opertor返回一個構造的矩陣 Mat(const Mat& m, const Rect& roi);這個建構函式返回引用m矩陣中roi那部分感興趣的範圍 258 //最終結果是:prev_color矩陣元素拷貝到了 img_show矩陣對應Rect興趣區域 因為img_show 是一個2*row行 cols列 可以包含兩個prev_color矩陣 259 prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) );//0列 0行 ->cols列 rows行 大小 //實際上就是把第一幀的影象拷貝到img_show中 260 //因為我們針對每一幀影象都會把第一幀影象拷貝到這裡 所以這裡實際上執行一次即可 261 //可以修改 前加上僅僅對第二幀執行一次即可 262 color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) );//0列 rows行 ->cols列 rows行 大小 263 264 //在measurements容器中 隨機挑選出符合要求的測量值 在img_show矩陣中對應部分進行標記（因為img_show上半部分是第一幀影象，下半部分是當前影象） 265 for ( Measurement m:measurements ) 266 { 267 if ( rand() > RAND_MAX/5 ) 268 continue; 269 Eigen::Vector3d p = m.pos_world; 270 Eigen::Vector2d pixel_prev = project3Dto2D ( float( p ( 0,0 ) ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );//世界座標系下的 影象座標2D 271 Eigen::Vector3d p2 = Tcw*m.pos_world;//將空間點轉換到下一幀相機座標系下 272 Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );//當前幀座標系下的影象畫素座標 273 //對於超出下一幀影象畫素座標軸範圍的點 捨棄不畫 274 if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows ) 275 continue; 276 //隨機獲取bgr顏色 在cv::circle中 為關鍵點用不同的顏色圓來畫出 277 float b = 255*float ( rand() ) /RAND_MAX; 278 float g = 255*float ( rand() ) /RAND_MAX; 279 float r = 255*float ( rand() ) /RAND_MAX; 280 //在img_show包含兩幀影象上 以關鍵點為圓心畫圓 半徑為8個畫素 顏色為bgr隨機組合 2表示外輪廓線寬度為2 如果為負數則表示填充圓 281 //pixel_prev 都是世界座標系下的座標 (以第一幀為參考系) 和當前幀下的對比 可以看出關鍵點的數量會逐漸減少 282 cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 8, cv::Scalar ( b,g,r ), 2 ); 283 cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 8, cv::Scalar ( b,g,r ), 2 );//注意這裡+color.rows 當前幀在img_show的下半部分 284 //連線前後兩針匹配好的點 285 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 ( 0,0,250 ), 1 ); 286 } 287 cv::imshow ( "result", img_show ); 288 cv::waitKey ( 0 ); 289 290 } 291 return 0; 292 } 293 294 void poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw ) 295 { 296 // 初始化g2o 297 typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock; // 求解的向量是6＊1的 298 DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType >(); 299 DirectBlock* solver_ptr = new DirectBlock(unique_ptr<DirectBlock::LinearSolverType>(linearSolver)); 300 g2o::OptimizationAlgorithmLevenberg *solver =new g2o::OptimizationAlgorithmLevenberg(unique_ptr<DirectBlock>(solver_ptr)); 301 // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N 302 303 g2o::SparseOptimizer optimizer; 304 optimizer.setAlgorithm ( solver ); 305 optimizer.setVerbose( true ); 306 307 auto pose = new g2o::VertexSE3Expmap(); 308 pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) ); 309 pose->setId ( 0 ); 310 optimizer.addVertex ( pose ); 311 312 // 新增邊 313 int id=1; 314 for ( Measurement m: measurements ) 315 { 316 auto edge = new EdgeSE3ProjectDirect ( 317 m.pos_world, 318 K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray 319 ); 320 edge->setVertex ( 0, pose );//設定一元邊連結的頂點 321 edge->setMeasurement ( m.grayscale );//設定測量值 即把前一幀關鍵點的灰度值作為測量值 供給下一幀進行匹配計算RT 322 edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );//因為誤差維度是1 所以信心矩陣為1x1 323 edge->setId ( id++ ); 324 optimizer.addEdge ( edge ); 325 } 326 cout<<"edges in graph: "<<optimizer.edges().size() <<endl;//邊的個數 實際上反應了關鍵點的個數 327 optimizer.initializeOptimization(); 328 optimizer.optimize ( 30 ); 329 Tcw = pose->estimate(); 330 } 331 332 333 //獲取小塊平均灰度值 334 // gray:灰度矩陣 x,y表示以(x,y)為中心 計算的小塊的平均灰度 335 float getPatchAverageGray(const cv::Mat &gray ,float u, float v){ 336 int x = cvRound(u); 337 int y = cvRound(v); 338 339 float grayscale = 0.0; 340 341 for (int j = y-1; j <= y+1 ; ++j) 342 for(auto i = x-1;i<= x+1; ++i){ 343 grayscale += float ( gray.ptr<uchar> (j)[i] ); 344 } 345 //grayscale/= ( (2*patchRadius + 1)*(2*patchRadius +1) ); 346 grayscale/=9; 347 return grayscale; 348 }
4.使用Ceres實現RGB-D上的稀疏直接法和半稠密直接法。