版權聲明：本文為博主原創文章，轉載請注明出處: http://www.cnblogs.com/newneul/p/8571653.html
3、題目回顧:
在稀疏直接法中，假設單個像素周圍小塊的光度也不變，是否可以提高算法的健壯性？請編程實現、
分析：根據直接法的思想：基於灰度不變假設。因為題目假設了周圍小塊光度也不變，那么我們可以用單個像素周圍的3x3或5x5小塊的平均灰度值作為單個像素的灰度值，從一定程度上調高了健壯性，但是效果提升有限。
下面程序集成了direct_sparse.cpp程序的解釋和利用半稠密直接法思路結合稀疏直接法以及本題小塊思想的結合。共有四種配置方案。默認為稀疏直接法+小塊思路 也就是本題目的解法。當然也可以對應修改配置信息。執行相應的功能查看效果。
程序代碼如下:
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 26 //下面兩種配置參數 可以任意組合 互不沖突 下面列出4種情況: 27 /* SEMIDENSE BookExercise 28 * 0 0 : 采用direct_sparse 例子 稀疏直接法（提取FAST特征） 29 * 0 1 : 采用書上課后習題3（小塊均值法）+ 稀疏直接法（提取FAST特征） 30 * 1 0 : 采用稀疏直接法(提取FAST特征) +半稠密直接法（選取梯度大的） 31 * 1 1 : 采用半稠密直接法（提取FAST特征）+課后習題3思路（小塊均值法）+稀疏直接法（提取FAST特征） 32 * 從枚舉器中選擇合適的值，對應修改下面的兩個值: 33 * GradientThread 梯度域值 34 * PATCH_RADIUS 小塊半徑 35 * */ 36 #define SEMIDENSE 0 // 1 (稀疏直接法+半稠密法) 表示利用半稠密方法思想 篩選梯度比較大的點 這里就是從FAST關鍵點中篩選出梯度大的點 37 // 0 表示僅僅用稀疏直接法 38 #define BookExercise 1 // 1 表示運行課后習題3的做法 39 // 0 表示不采用課后習題3的做法 40 41 #if SEMIDENSE 42 enum GradientThreadChoice{ 43 GRADIENT_10 = 10, //1170點 44 GRADIENT_15 = 15, //984點 45 GRADIENT_20 = 20, //805點 46 GRADIENT_25 = 25, //656點 47 GRADIENT_30 = 30, //514點 48 GRADIENT_50 = 50, //201點 49 GRADIENT_100 = 100 //33點 50 }; 51 #define GradientThread GRADIENT_50 // 默認篩選的梯度域值,通過調節域值（默認50） 可以增加關鍵點的個數 52 53 #endif 54 55 #if BookExercise 56 enum PatchRadiusChoices{ // 塊大小選取類 57 PATCH_RADIUS_ONE = 1, // 表示以像素為圓心 半徑為1大小的塊 58 PATCH_RADIUS_TWO = 2 // 最多半徑為2 否則計算量太大（因為邊計算誤差函數會進行插值查找 塊越大 計算量成平方增加） 59 }; 60 PatchRadiusChoices PATCH_RADIUS = PATCH_RADIUS_ONE; //將全局變量置為該選項半徑為1 61 #endif // 1對應3x3小塊 2對應5x5小塊 62 63 /*+++++++++++++++++++++++++++END參數配置取++++++++++++++++++++++++++++++++++++=*/ 64 65 /******************************************** 66 * 本節演示了RGBD上的稀疏直接法 67 ********************************************/ 68 //獲取小塊平均灰度值 69 // gray:灰度矩陣 x,y表示以(x,y)為中心 計算的小塊的平均灰度 patchRadius 表示塊的半徑 70 #if BookExercise 71 float getPatchAverageGray(const cv::Mat &gray ,float u, float v,int patchRadius); 72 // 一次測量的值，包括一個世界坐標系下三維點（以第一幀為參考系）與一個灰度值（以第一幀為參考的3D點對應灰度圖像的灰度值，灰度圖是由color圖像轉換到對應的gray圖像得到的 ） 73 #endif 74 struct Measurement 75 { 76 Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {} 77 Eigen::Vector3d pos_world; 78 float grayscale; 79 }; 80 81 //轉換成相機坐標系下坐標 82 inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale ) 83 { 84 float zz = float ( d ) /scale; 85 float xx = zz* ( x-cx ) /fx; 86 float yy = zz* ( y-cy ) /fy; 87 return Eigen::Vector3d ( xx, yy, zz ); 88 } 89 90 inline Eigen::Vector2d project3Dto2D ( float x, float y, float z, float fx, float fy, float cx, float cy ) 91 { 92 float u = fx*x/z+cx; 93 float v = fy*y/z+cy; 94 return Eigen::Vector2d ( u,v ); 95 } 96 97 // 直接法估計位姿 98 // 輸入：測量值（空間點的灰度），新的灰度圖，相機內參； 輸出：相機位姿 99 // 返回：true為成功，false失敗 這里並沒有設置返回值信息！ 100 bool poseEstimationDirect ( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw ); 101 102 // project a 3d point into an image plane, the error is photometric error 103 // an unary edge with one vertex SE3Expmap (the pose of camera) 104 //誤差值維度 誤差類型 頂點類型 105 class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap> 106 { 107 public: 108 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 109 110 EdgeSE3ProjectDirect() = default; //代替下面的方式 默認產生合成的構造函數 111 //EdgeSE3ProjectDirect(){} 112 EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image ) 113 : x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image ) //灰度圖像指針 114 {} 115 116 virtual void computeError()override 117 { 118 const VertexSE3Expmap* v =static_cast<const VertexSE3Expmap*> ( _vertices[0] ); 119 Eigen::Vector3d x_local = v->estimate().map ( x_world_ ); 120 float x = x_local[0]*fx_/x_local[2] + cx_; //世界坐標轉換到當期幀像素坐標 121 float y = x_local[1]*fy_/x_local[2] + cy_; 122 // check x,y is in the image 123 //距離圖像四條邊4個像素大小的區域內作為有效投影區域 對於不在該范圍內的點誤差值設為0 為了防止計算的誤差太大 拉低內點對誤差的影響 導致估計的RT嚴重偏離真值 124 if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows ) 125 { 126 _error ( 0,0 ) = 0.0; 127 this->setLevel ( 1 );//??????????????????????????????????????????????????? 128 } 129 else 130 { 131 #if BookExercise //表示運行課后習題3 132 //選取小塊的大小為2x2 133 float sumValue = 0.0; 134 for(int i = x-PATCH_RADIUS ; i<= x+PATCH_RADIUS ; ++i) 135 for (int j = y-PATCH_RADIUS; j <= y+PATCH_RADIUS ; ++j) { 136 sumValue += getPixelValue(i,j); 137 } 138 sumValue /=( (2*PATCH_RADIUS +1)*(2*PATCH_RADIUS+1) ); //求得元素周圍小塊的平均灰度值 139 _error (0,0) = sumValue - _measurement; 140 #else 141 _error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;//經過在灰度圖中插值獲得的像素值 減去測量值 142 #endif 143 } 144 } 145 146 // plus in manifold 147 //提供誤差關於位姿的雅克比矩陣 書上8.16式子 只不過負號去掉了 因為用的是當前幀灰度值 - 世界坐標下的測量值 148 virtual void linearizeOplus( )override 149 { 150 if ( level() == 1 ) 151 { 152 _jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero(); 153 return; 154 } 155 VertexSE3Expmap* vtx = dynamic_cast<VertexSE3Expmap*> ( _vertices[0] ); 156 Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ ); // q in book 轉換到第二幀坐標系下 157 158 double x = xyz_trans[0]; 159 double y = xyz_trans[1]; 160 double invz = 1.0/xyz_trans[2]; 161 double invz_2 = invz*invz; 162 163 float u = x*fx_*invz + cx_;//投影到第二幀像素坐標系 164 float v = y*fy_*invz + cy_; 165 166 // jacobian from se3 to u,v 167 // NOTE that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation 168 Eigen::Matrix<double, 2, 6> jacobian_uv_ksai; 169 170 //書上8.15式子 171 jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_; 172 jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_; 173 jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_; 174 jacobian_uv_ksai ( 0,3 ) = invz *fx_; 175 jacobian_uv_ksai ( 0,4 ) = 0; 176 jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_; 177 178 jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_; 179 jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_; 180 jacobian_uv_ksai ( 1,2 ) = x*invz *fy_; 181 jacobian_uv_ksai ( 1,3 ) = 0; 182 jacobian_uv_ksai ( 1,4 ) = invz *fy_; 183 jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_; 184 185 Eigen::Matrix<double, 1, 2> jacobian_pixel_uv; 186 187 //書上I2對像素坐標系的偏導數 這里很有可能 計算出來的梯度為0 因為FAST角點的梯度沒有限制 188 //這也是半稠密法主要改進的地方 就是選關鍵點的時候 選擇梯度大的點 因此這里的梯度就不可能為0了 189 jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2; 190 jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2; 191 192 _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;//書上8.16式子 193 } 194 195 // dummy read and write functions because we don't care... 196 virtual bool read ( std::istream& in ) {} 197 virtual bool write ( std::ostream& out ) const {} 198 199 protected: 200 // get a gray scale value from reference image (bilinear interpolated) 201 //cv::Mat中成員變量代表的含義:http://blog.csdn.net/dcrmg/article/details/52294259 202 //下面的方式 針對單通道的灰度圖 203 inline float getPixelValue ( float x, float y )//通過雙線性插值獲取浮點坐標對應的插值后的像素值 204 { 205 uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];//step表示圖像矩陣一行的所有字節（包括所有通道的總和），data表示存儲圖像的開始指針 206 float xx = x - floor ( x ); //取整函數 207 float yy = y - floor ( y ); 208 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) 209 //這里的xx 就是u yy就是v 210 ( 1-xx ) * ( 1-yy ) * data[0] + 211 xx* ( 1-yy ) * data[1] + 212 ( 1-xx ) *yy*data[ image_->step ] + //I(i+1,j) //這里相當於像素的周期是image_->step，即每一行存儲像素的個數為image_->step 213 xx*yy*data[image_->step+1] //I(i+1,j+1) //data[image_->step]是I(i,j)對應的下一行像素為I(i+1,j) 214 ); 215 } 216 public: 217 Eigen::Vector3d x_world_; // 3D point in world frame 218 float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics 219 cv::Mat* image_=nullptr; // reference image 220 }; 221 222 int main ( int argc, char** argv ) 223 { 224 if ( argc != 2 ) 225 { 226 cout<<"usage: useLK path_to_dataset"<<endl; 227 return 1; 228 } 229 srand ( ( unsigned int ) time ( 0 ) ); 230 string path_to_dataset = argv[1]; 231 string associate_file = path_to_dataset + "/associate.txt"; 232 233 ifstream fin ( associate_file ); 234 235 string rgb_file, depth_file, time_rgb, time_depth; 236 cv::Mat color, depth, gray; 237 vector<Measurement> measurements;//Measurement類 存儲世界坐標點（以第一幀為參考的FAST關鍵點） 和 對應的灰度圖像（由color->gray）的灰度值 238 // 相機內參 239 float cx = 325.5; 240 float cy = 253.5; 241 float fx = 518.0; 242 float fy = 519.0; 243 float depth_scale = 1000.0; 244 Eigen::Matrix3f K; 245 K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f; 246 247 Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();//三維變換矩陣T 4X4 初始時刻是單位R矩陣+0平移向量 248 249 cv::Mat prev_color; 250 // 我們以第一個圖像為參考，對后續圖像和參考圖像做直接法 ，每一副圖像 都會與第一幀圖像做直接法計算第一幀到當前幀的RT 但是經過更多的幀后 關鍵點的數量會減少， 251 //所以實際應用時 應當規定關鍵點的數量少於多少 就該從新設定參考系，再次利用直接法 ，但是會累計的誤差需要解決？？？？ 252 for ( int index=0; index<10; index++ )//總共10幀 253 { 254 cout<<"*********** loop "<<index<<" ************"<<endl; 255 fin>>time_rgb>>rgb_file>>time_depth>>depth_file; 256 color = cv::imread ( path_to_dataset+"/"+rgb_file ); 257 depth = cv::imread ( path_to_dataset+"/"+depth_file, -1 );//-1 按原圖像的方式存儲 detph 16位存儲 258 if ( color.data==nullptr || depth.data==nullptr ) 259 continue; 260 //轉換后的灰度圖為g2o優化需要的邊提供灰度值 261 cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY ); //將顏色圖3通道 轉換為灰度圖單通道 8位無符號 對應邊類的雙線性插值計算放法以單通道計算的 262 263 //第一幀為世界坐標系 計算FAST關鍵點 為之后與當前幀用直接法計算RT做准備 264 if ( index ==0 )//以第一幀為參考系 計算關鍵點后存儲測量值（關鍵點對應的灰度值） 以此為基准跟蹤后面的圖像 計算位姿 265 { 266 // 對第一幀提取FAST特征點 267 vector<cv::KeyPoint> keypoints; 268 cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create(); 269 detector->detect ( color, keypoints ); 270 //對於2D關鍵點獲取 3D信息 並去掉范圍外的點 存儲符合要求的關鍵點的深度值和3D信息 271 //對所有關鍵點挑選出符合要求且有深度值的 存儲到vector<Measurement> measurements中 為g2o邊提供灰度測量值和空間點坐標 272 for ( auto kp:keypoints ) 273 { 274 #if SEMIDENSE //表示利用半稠密法的思想 只不過結果由原來的1402個點 變為了201個點 特征點數目降低了 但是看起來精度還是很高 可以適當調整梯度域值 275 Eigen::Vector2d delta ( //計算像素坐標系下 兩個方向的變化量 276 gray.ptr<uchar>(int ( kp.pt.y))[ int(kp.pt.x+1)] - gray.ptr<uchar>(int(kp.pt.y))[int(kp.pt.x-1)], 277 gray.ptr<uchar>(int(kp.pt.y+1))[int(kp.pt.x)] - gray.ptr<uchar>(int(kp.pt.y-1))[int(kp.pt.x)] 278 ); 279 //cout<<" keypoints坐標值: "<<kp.pt.x<<" "<<kp.pt.y<<endl;//可以看出點雖然存儲方式是浮點數 但是實際的值都是int類型 280 if ( delta.norm() < GradientThread )//可以轉變為變化量的2范數小於50/16 默認域值為30 可調 281 continue; 282 #endif 283 // 去掉鄰近邊緣處的點 在離圖像四條邊20個像素構成的內矩陣范圍內是符合要求的關鍵點 284 if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows ) 285 continue; 286 //depth.ptr<ushort>( kp.pt.y)獲取行指針 cvRound(kp.pt,y) 表示返回跟參數值最接近的整數值 因為像素量化后是整數，而kp.pt.y存儲方式是float，所以強制轉換一下即可 287 ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];//16位深度圖 288 if ( d==0 ) 289 continue; 290 Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale ); //3D相機坐標系（第一幀 也是世界幀） 291 #if BookExercise //計算小塊平均灰度值作為對應單一像素的測量值 增加算法健壯性 292 float grayscale = getPatchAverageGray( gray , kp.pt.x , kp.pt.y , PATCH_RADIUS_ONE ); 293 #else //否則是正常以單個像素的灰度值作為測量值 294 float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] ); //8位無符號也取整數 因為灰度圖 是對應整數（int）的像素的 kp.pt.y是float 295 #endif 296 measurements.push_back ( Measurement ( p3d, grayscale ) ); 297 } 298 prev_color = color.clone(); //深拷貝color圖像 299 continue; 300 } 301 // 使用直接法計算相機運動 302 //從第二幀開始計算相機位姿g2o優化 303 chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); 304 //優化過程中要提供灰度圖像 邊里面計算誤差函數需要 為getPixelValue()該函數提供灰度值查找 305 poseEstimationDirect ( measurements, &gray, K, Tcw );//Tcw為世界坐標到下一幀坐標的累計值 最后Tcw的結果是從世界坐標 到當前幀下的轉換 306 chrono::steady_clock::time_point t2 = chrono::steady_clock::now(); 307 chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 ); 308 cout<<"direct method costs time: "<<time_used.count() <<" seconds."<<endl; 309 cout<<"Tcw="<<Tcw.matrix() <<endl; 310 311 // plot the feature points 312 cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 );//目的是為了之后對比前后兩幀圖像的關鍵點數量 所以建立一個可以存儲pre_color 和color 大小的矩陣 313 //Rect(參數)表示坐標0,0 到cols,rows 那么大的矩形 314 //img_show.operator(const Rect &roi 參數:表示這個矩陣的某個興趣區域) 315 //img_show.opertor返回一個構造的矩陣 Mat(const Mat& m, const Rect& roi);這個構造函數返回引用m矩陣中roi那部分感興趣的范圍 316 //最終結果是:prev_color矩陣元素拷貝到了 img_show矩陣對應Rect興趣區域 因為img_show 是一個2*row行 cols列 可以包含兩個prev_color矩陣 317 prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) );//0列 0行 ->cols列 rows行 大小 //實際上就是把第一幀的圖像拷貝到img_show中 318 //因為我們針對每一幀圖像都會把第一幀圖像拷貝到這里 所以這里實際上執行一次即可 319 //可以修改 前加上僅僅對第二幀執行一次即可 320 color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) );//0列 rows行 ->cols列 rows行 大小 321 322 //在measurements容器中 隨機挑選出符合要求的測量值 在img_show矩陣中對應部分進行標記（因為img_show上半部分是第一幀圖像，下半部分是當前圖像） 323 for ( Measurement m:measurements ) 324 { 325 if ( rand() > RAND_MAX/5 ) 326 continue; 327 Eigen::Vector3d p = m.pos_world; 328 Eigen::Vector2d pixel_prev = project3Dto2D ( float( p ( 0,0 ) ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );//世界坐標系下的 圖像坐標2D 329 Eigen::Vector3d p2 = Tcw*m.pos_world;//將空間點轉換到下一幀相機坐標系下 330 Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );//當前幀坐標系下的圖像像素坐標 331 //對於超出下一幀圖像像素坐標軸范圍的點 舍棄不畫 332 if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows ) 333 continue; 334 //隨機獲取bgr顏色 在cv::circle中 為關鍵點用不同的顏色圓來畫出 335 float b = 255*float ( rand() ) /RAND_MAX; 336 float g = 255*float ( rand() ) /RAND_MAX; 337 float r = 255*float ( rand() ) /RAND_MAX; 338 //在img_show包含兩幀圖像上 以關鍵點為圓心畫圓 半徑為8個像素 顏色為bgr隨機組合 2表示外輪廓線寬度為2 如果為負數則表示填充圓 339 //pixel_prev 都是世界坐標系下的坐標 (以第一幀為參考系) 和當前幀下的對比 可以看出關鍵點的數量會逐漸減少 340 cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 8, cv::Scalar ( b,g,r ), 2 ); 341 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的下半部分 342 //連接前后兩針匹配好的點 343 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 ); 344 } 345 cv::imshow ( "result", img_show ); 346 cv::waitKey ( 0 ); 347 348 } 349 return 0; 350 } 351 352 bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw ) 353 { 354 // 初始化g2o 355 typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock; // 求解的向量是6＊1的 因為一元邊 所以后面的1可以是其他的數字 356 auto linearSolver = g2o::make_unique<g2o::LinearSolverDense< DirectBlock::PoseMatrixType >>(); 357 auto solver_ptr = g2o::make_unique<DirectBlock>( std::move(linearSolver) ); 358 g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr) ); 359 // DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > (); 360 // DirectBlock* solver_ptr = new DirectBlock ( linearSolver ); 361 // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N 362 // g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M 363 g2o::SparseOptimizer optimizer; 364 optimizer.setAlgorithm ( solver ); 365 optimizer.setVerbose( true ); 366 367 auto pose = new g2o::VertexSE3Expmap(); 368 pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) ); 369 pose->setId ( 0 ); 370 optimizer.addVertex ( pose ); 371 372 // 添加邊 373 int id=1; 374 for ( Measurement m: measurements ) 375 { 376 auto edge = new EdgeSE3ProjectDirect ( 377 m.pos_world, 378 K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray 379 ); 380 edge->setVertex ( 0, pose );//設置一元邊鏈接的頂點 381 edge->setMeasurement ( m.grayscale );//設置測量值 即把前一幀關鍵點的灰度值作為測量值 供給下一幀進行匹配計算RT 382 edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );//因為誤差維度是1 所以信心矩陣為1x1 383 edge->setId ( id++ ); 384 optimizer.addEdge ( edge ); 385 } 386 cout<<"edges in graph: "<<optimizer.edges().size() <<endl;//邊的個數 實際上反應了關鍵點的個數 387 optimizer.initializeOptimization(); 388 optimizer.optimize ( 30 ); 389 Tcw = pose->estimate(); 390 } 391 392 #if BookExercise 393 //獲取小塊平均灰度值 394 // gray:灰度矩陣 x,y表示以(x,y)為中心 計算的小塊的平均灰度 patchSize 表示塊的半徑 395 float getPatchAverageGray(const cv::Mat &gray ,float u, float v,int patchRadius){ 396 int x = cvRound(u); 397 int y = cvRound(v); 398 if( (patchRadius < 0) || ( (2*patchRadius+1) > 5 ) ){ 399 std::cout<<"Error:請修改PATCH_RADIUS為指定值1 or 2! "<<std::endl; 400 exit(1); 401 } 402 float grayscale = 0.0; 403 // y - patchRadius;//代表y坐標 404 // x - patchRadius;//代表x坐標 405 for (int j = y-patchRadius; j <= y+patchRadius ; ++j) 406 for(auto i = x-patchRadius;i<= (x+patchRadius); ++i){ 407 grayscale += float ( gray.ptr<uchar> (j)[i] ); 408 } 409 grayscale/= ( (2*patchRadius + 1)*(2*patchRadius +1) ); 410 return grayscale; 411 } 412 #endif
歡迎大家關注我的微信公眾號「佛系師兄」，里面有關於 Ceres 以及 OpenCV 等更多技術文章。
比如
「反復研究好幾遍，我才發現關於 CMake 變量還可以這樣理解！」
更多好的文章會優先在里面不定期分享！打開微信客戶端，掃描下方二維碼即可關注！
