版權聲明：本文為博主原創文章，轉載請注明出處: http://www.cnblogs.com/newneul/p/8545450.html
6、在PnP優化中，將第一個相機的觀測也考慮進來，程序應如何書寫？最后結果會有何變化？
分析:實際上在PnP例子中，我們可以把第一幀作為世界坐標系，然后在優化過程中對於第一幀的RT我們不做優化，但是我們在添加節點時仍然要將第一幀在世界坐標系下的空間點加入到圖中，並且與第一幀的位姿鏈接起來，然后將第一幀坐標系下的空間點與第二幀的位姿連接起來。
下面是我們修改的部分(節點和邊的添加過程)：
1 //向優化器中添加節點和邊 2 //添加節點 Vertex 3 //（1）添加位姿節點 第一幀作為世界坐標系（不優化） 同時也是相機坐標系 4 int poseVertexIndex = 0; //位姿節點索引為0 總共兩個位姿態節點 第一幀和第二幀 5 Eigen::Matrix3d R2Init; 6 R2Init << 7 R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ) , 8 R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ) , 9 R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 ) ; 10 for( ; poseVertexIndex < PoseVertexNum ; poseVertexIndex++ ){ 11 auto pose = new g2o::VertexSE3Expmap(); //相機位姿 12 pose->setId( poseVertexIndex ); //設置節點標號 13 pose->setFixed( poseVertexIndex == 0 ); //如果是第一幀 則固定住 不優化 14 if( poseVertexIndex == 1 ) //第二幀時讓RT估計值為pnp得到的值 加快優化速度! 15 pose->setEstimate( 16 g2o::SE3Quat( R2Init, 17 Eigen::Vector3d( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) ) 18 ) 19 ); //兩幀圖像的位姿預設值都為 r=單位矩陣 t=0(當然這里可以填寫自己設定的預設值 比如Pnp估計值) 20 else 21 pose->setEstimate( g2o::SE3Quat() ); 22 optimizer.addVertex( pose ); //位姿節點加入優化器 23 } 24 //（2）添加路標節點 25 int landmarkVertexIndex = PoseVertexNum ; 26 for( int i = 0; i < points1_3d.size() ; i ++ ){ 27 auto point = new g2o::VertexSBAPointXYZ(); //路標點 28 point->setId( landmarkVertexIndex + i ); //設置路標點節點標號 29 point->setMarginalized( true ); //設置邊緣化 30 point->setEstimate( Eigen::Vector3d( points1_3d[i].x, points1_3d[i].y, points1_3d[i].z ) ); //設置估計值 實際上就是第一幀坐標下的3d點坐標（也是世界坐標系下的坐標） 31 optimizer.addVertex( point ); //路標節點加入優化器 32 } 33 //加入相機參數（當然有另一種方式：查看筆記兩種邊的不同點）(最后一項為0 默認fx = fy 然后優化位姿 與g2o::EdegeSE3ProjectXYZ不同 筆記以記載 ) 34 g2o::CameraParameters *camera = new g2o::CameraParameters( 35 K.at< double >(0,0), Eigen::Vector2d( K.at< double >(0,2), K.at< double >(1,2) ),0 36 ); 37 camera->setId( 0 ); 38 optimizer.addParameter( camera ); 39 40 //加入邊edge 41 //世界坐標下路標點連接到第一幀位姿節點（因為以第一幀坐標系當做世界坐標系 所以我們前面沒有優化第一幀的RT 僅僅優化第一幀到第二幀的RT） 42 for(int i= 0 ;i < points1_2d.size() ; i++ ){ 43 auto edge = new g2o::EdgeProjectXYZ2UV(); //設置連接到第一幀的邊 44 //二元邊 連接節點 45 edge->setVertex( 0, dynamic_cast< g2o::VertexSBAPointXYZ * >( optimizer.vertex( landmarkVertexIndex + i ) ) ); //世界坐標系下的路標節點 46 edge->setVertex( 1, dynamic_cast< g2o::VertexSE3Expmap * >( optimizer.vertex(0) ) ); 47 edge->setMeasurement( Eigen::Vector2d ( points1_2d[i].x, points1_2d[i].y ) ); //設置測量值為第一幀下的相機歸一化平面坐標 48 edge->setParameterId(0,0); //最后一位設置使用的相機參數(因為上面僅僅輸入了一個相機參數id=0, 對應上面camer->setId(0),第一個參數0不知道是什么，但是必須為0) 49 edge->setInformation ( Eigen::Matrix2d::Identity() ); //信息矩陣2x2的單位陣 50 optimizer.addEdge( edge ); 51 } 52 //第一幀路標點鏈接到第二幀位姿節點 53 for( int i=0 ;i < points1_2d.size() ; i++){ 54 auto edge = new g2o::EdgeProjectXYZ2UV(); //設置鏈接到第二幀的邊 55 edge->setVertex( 0, dynamic_cast< g2o::VertexSBAPointXYZ * >( optimizer.vertex( landmarkVertexIndex + i) ) ); //第一幀坐標系下的路標點 56 edge->setVertex( 1, dynamic_cast< g2o::VertexSE3Expmap *> ( optimizer.vertex(1) ) ); //連接到第二個位姿節點 57 edge->setMeasurement( Eigen::Vector2d ( points2_2d[i].x, points2_2d[i].y ) ); //設置測量值為第二幀下的相機歸一化平面坐標 58 edge->setInformation( Eigen::Matrix2d::Identity() ); //信息矩陣為2x2 實際上就是誤差的加權為1：1的 59 edge->setParameterId(0,0); 60 optimizer.addEdge( edge ); 61 }
需要注意的是代碼本身集成了PnP 3d_2d那節例子和課后習題6題，可以自己通過下面的選擇決定是否運行
#define MyselfBAFunc 1 //1 課后習題6需要的BA優化函數（運行課后習題6對應的程序） //0 例程用的（PNP3d_2d對應的程序）
完整代碼如下:
1 #include <iostream> 2 #include <opencv2/core/core.hpp> 3 #include <opencv2/features2d/features2d.hpp> 4 #include <opencv2/highgui/highgui.hpp> 5 #include <opencv2/calib3d/calib3d.hpp> 6 #include <Eigen/Core> 7 #include <Eigen/Geometry> 8 #include <g2o/core/base_vertex.h> 9 #include <g2o/core/base_unary_edge.h> 10 #include <g2o/core/block_solver.h> 11 #include <g2o/core/optimization_algorithm_levenberg.h> 12 #include <g2o/solvers/csparse/linear_solver_csparse.h> 13 #include <g2o/types/sba/types_six_dof_expmap.h> 14 #include <chrono> 15 using namespace std; 16 using namespace cv; 17 #define MyselfBAFunc 1 //1 課后習題6需要的BA優化函數 18 //0 例程用的 19 void find_feature_matches ( 20 const Mat& img_1, const Mat& img_2, 21 std::vector<KeyPoint>& keypoints_1, 22 std::vector<KeyPoint>& keypoints_2, 23 std::vector< DMatch >& matches ); 24 25 // 像素坐標轉相機歸一化坐標 26 Point2d pixel2cam ( const Point2d& p, const Mat& K ); 27 #if MyselfBAFunc 28 void MyselfBAFun( 29 const vector< cv::Point3f> &points1_3d, //第一幀3d點(匹配好且有深度信息的點) 30 const vector< cv::Point2f> &points1_2d, //第一幀像素平面2d點(匹配好的點) 31 const vector< cv::Point2f> &points2_2d, //第二幀像素平面2d點(匹配好的點) 32 const Mat&K, //因為里面沒有修改相應的值所以用const 33 const Mat&R, 34 const Mat&t 35 ); 36 #else 37 void bundleAdjustment ( 38 const vector<Point3f> &points_3d, 39 const vector<Point2f> &points_2d, 40 const Mat& K, 41 Mat& R, Mat& t 42 ); 43 #endif 44 int main ( int argc, char** argv ) 45 { 46 if ( argc != 5 ) 47 { 48 cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl; 49 return 1; 50 } 51 //-- 讀取圖像 52 Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR ); 53 Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR ); 54 55 vector<KeyPoint> keypoints_1, keypoints_2; 56 vector<DMatch> matches; 57 find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches ); 58 cout<<"一共找到了"<<matches.size() <<"組匹配點"<<endl; 59 60 // 建立3D點 61 Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );// 深度圖為16位無符號數，單通道圖像 CV_LOAD_IMAGE_UNCHANGED表示讀取原始圖像 62 Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); 63 vector<Point3f> pts_3d; 64 vector<Point2f> pts_2d; 65 #if MyselfBAFunc 66 vector<Point2f> pts1_2d; //第一幀下的像素坐標 67 #endif 68 for ( DMatch m:matches ) 69 { 70 //可以參考書上101頁對應的程序，表示獲取對應位置的深度圖片的深度值 71 ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ]; 72 if ( d == 0 ) // bad depth 73 continue; 74 float dd = d/5000.0; 75 Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K ); 76 pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );//表示的是 相機坐標系下的3D坐標 //這里是通過RGBD獲取的深度信息，但是我們可以用三角測量獲得第一幀下的3D坐標 77 pts_2d.push_back ( keypoints_2[m.trainIdx].pt ); //第二幀 匹配好的點 2D像素坐標 78 #if MyselfBAFunc 79 pts1_2d.push_back( keypoints_1[m.queryIdx].pt ); //第一幀 匹配好的點 2D像素坐標 80 #endif 81 } 82 //上面已經獲得了第一幀坐標系的下的3d坐標 相當於世界坐標系下的坐標 （因為僅僅有兩針圖像幀 所以 以第一幀為世界坐標，也就是說 世界坐標到第一幀圖像的R=I T=0 ） 83 cout<<"3d-2d pairs: "<<pts_3d.size() <<endl; 84 Mat r, t; //定義旋轉和平移變量 85 //參數信息: 第一幀3D 第二幀像素2D 內參矩陣k 無失真補償 旋轉向量r 平移向量t false表示輸入的r t不作為初始化值 如果是true則此時會把t r作為初始值進行迭代 86 solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false,SOLVEPNP_EPNP ); // 調用OpenCV 的 PnP 求解，可選擇EPNP，DLS等方法SOLVEPNP_EPNP 87 Mat R; 88 cv::Rodrigues ( r, R ); // r為旋轉向量形式，用Rodrigues公式轉換為矩陣 89 90 cout<<"R="<<endl<<R<<endl; 91 cout<<"t="<<endl<<t<<endl; 92 cout<<"calling bundle adjustment"<<endl; 93 #if MyselfBAFunc 94 MyselfBAFun( pts_3d, pts1_2d, pts_2d, K, R, t); 95 #else 96 bundleAdjustment ( pts_3d, pts_2d, K, R, t ); 97 #endif 98 99 } 100 101 void find_feature_matches ( const Mat& img_1, const Mat& img_2, 102 std::vector<KeyPoint>& keypoints_1, 103 std::vector<KeyPoint>& keypoints_2, 104 std::vector< DMatch >& matches ) 105 { 106 //-- 初始化 107 Mat descriptors_1, descriptors_2; 108 // used in OpenCV3 109 Ptr<FeatureDetector> detector = ORB::create(); 110 Ptr<DescriptorExtractor> descriptor = ORB::create(); 111 // use this if you are in OpenCV2 112 // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" ); 113 // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" ); 114 Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create ( "BruteForce-Hamming" ); 115 //-- 第一步:檢測 Oriented FAST 角點位置 116 detector->detect ( img_1,keypoints_1 ); 117 detector->detect ( img_2,keypoints_2 ); 118 119 //-- 第二步:根據角點位置計算 BRIEF 描述子 120 descriptor->compute ( img_1, keypoints_1, descriptors_1 ); 121 descriptor->compute ( img_2, keypoints_2, descriptors_2 ); 122 123 //-- 第三步:對兩幅圖像中的BRIEF描述子進行匹配，使用 Hamming 距離 124 vector<DMatch> match; 125 // BFMatcher matcher ( NORM_HAMMING ); 126 matcher->match ( descriptors_1, descriptors_2, match ); 127 128 //-- 第四步:匹配點對篩選 129 double min_dist=10000, max_dist=0; 130 131 //找出所有匹配之間的最小距離和最大距離, 即是最相似的和最不相似的兩組點之間的距離 132 for ( int i = 0; i < descriptors_1.rows; i++ ) 133 { 134 double dist = match[i].distance; 135 if ( dist < min_dist ) min_dist = dist; 136 if ( dist > max_dist ) max_dist = dist; 137 } 138 139 printf ( "-- Max dist : %f \n", max_dist ); 140 printf ( "-- Min dist : %f \n", min_dist ); 141 142 //當描述子之間的距離大於兩倍的最小距離時,即認為匹配有誤.但有時候最小距離會非常小,設置一個經驗值30作為下限. 143 for ( int i = 0; i < descriptors_1.rows; i++ ) 144 { 145 if ( match[i].distance <= max ( 2*min_dist, 30.0 ) ) 146 { 147 matches.push_back ( match[i] ); 148 } 149 } 150 } 151 152 Point2d pixel2cam ( const Point2d& p, const Mat& K ) 153 { 154 return Point2d 155 ( 156 ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ), 157 ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 ) 158 ); 159 } 160 161 #if MyselfBAFunc 162 void MyselfBAFun( 163 const vector< cv::Point3f> &points1_3d, //第一幀3d點(匹配好且有深度信息的點) 164 const vector< cv::Point2f> &points1_2d, //第一幀像素平面2d點(匹配好的點) 165 const vector< cv::Point2f> &points2_2d, //第二幀像素平面2d點(匹配好的點) 166 const Mat&K, //因為里面沒有修改相應的值所以用const 167 const Mat&R, 168 const Mat&t 169 ){ 170 #define PoseVertexNum 2 //定義位姿節點個數 本試驗僅僅有2幀圖 171 172 //設置優化器 173 typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block; //優化位姿6維 優化路標點3維 174 std::unique_ptr<Block::LinearSolverType> linearSolver=g2o::make_unique < g2o::LinearSolverCSparse<Block::PoseMatrixType> >();//線性求解設為CSparse 175 std::unique_ptr<Block> solver_ptr (new Block(std::move(linearSolver) ) ); 176 g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg( std::move(solver_ptr) ); 177 178 /* Block::LinearSolverType *linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); //設置線性求解器類型 179 Block *solver_ptr = new Block( std::unique_ptr<Block::LinearSolverType>(linearSolver) ); //矩陣塊求解器 180 g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg( std::unique_ptr<g2o::Solver>(solver_ptr) ); //LM優化算法 181 */ 182 g2o::SparseOptimizer optimizer; //設置稀疏優化器 183 optimizer.setAlgorithm(solver); //設置優化算法 184 185 //向優化器中添加節點和邊 186 //添加節點 Vertex 187 //（1）添加位姿節點 第一幀作為世界坐標系（不優化） 同時也是相機坐標系 188 int poseVertexIndex = 0; //位姿節點索引為0 總共兩個位姿態節點 第一幀和第二幀 189 Eigen::Matrix3d R2Init; 190 R2Init << 191 R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ) , 192 R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ) , 193 R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 ) ; 194 for( ; poseVertexIndex < PoseVertexNum ; poseVertexIndex++ ){ 195 auto pose = new g2o::VertexSE3Expmap(); //相機位姿 196 pose->setId( poseVertexIndex ); //設置節點標號 197 pose->setFixed( poseVertexIndex == 0 ); //如果是第一幀 則固定住 不優化 198 if( poseVertexIndex == 1 ) //第二幀時讓RT估計值為pnp得到的值 加快優化速度! 199 pose->setEstimate( 200 g2o::SE3Quat( R2Init, 201 Eigen::Vector3d( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) ) 202 ) 203 ); //兩幀圖像的位姿預設值都為 r=單位矩陣 t=0(當然這里可以填寫自己設定的預設值 比如Pnp估計值) 204 else 205 pose->setEstimate( g2o::SE3Quat() ); 206 optimizer.addVertex( pose ); //位姿節點加入優化器 207 } 208 //（2）添加路標節點 209 int landmarkVertexIndex = PoseVertexNum ; 210 for( int i = 0; i < points1_3d.size() ; i ++ ){ 211 auto point = new g2o::VertexSBAPointXYZ(); //路標點 212 point->setId( landmarkVertexIndex + i ); //設置路標點節點標號 213 point->setMarginalized( true ); //設置邊緣化 214 point->setEstimate( Eigen::Vector3d( points1_3d[i].x, points1_3d[i].y, points1_3d[i].z ) ); //設置估計值 實際上就是第一幀坐標下的3d點坐標（也是世界坐標系下的坐標） 215 optimizer.addVertex( point ); //路標節點加入優化器 216 } 217 //加入相機參數（當然有另一種方式：查看筆記兩種邊的不同點）(最后一項為0 默認fx = fy 然后優化位姿 與g2o::EdegeSE3ProjectXYZ不同 筆記以記載 ) 218 g2o::CameraParameters *camera = new g2o::CameraParameters( 219 K.at< double >(0,0), Eigen::Vector2d( K.at< double >(0,2), K.at< double >(1,2) ),0 220 ); 221 camera->setId( 0 ); 222 optimizer.addParameter( camera ); 223 224 //加入邊edge 225 //世界坐標下路標點連接到第一幀位姿節點（因為以第一幀坐標系當做世界坐標系 所以我們前面沒有優化第一幀的RT 僅僅優化第一幀到第二幀的RT） 226 for(int i= 0 ;i < points1_2d.size() ; i++ ){ 227 auto edge = new g2o::EdgeProjectXYZ2UV(); //設置連接到第一幀的邊 228 //二元邊 連接節點 229 edge->setVertex( 0, dynamic_cast< g2o::VertexSBAPointXYZ * >( optimizer.vertex( landmarkVertexIndex + i ) ) ); //世界坐標系下的路標節點 230 edge->setVertex( 1, dynamic_cast< g2o::VertexSE3Expmap * >( optimizer.vertex(0) ) ); 231 edge->setMeasurement( Eigen::Vector2d ( points1_2d[i].x, points1_2d[i].y ) ); //設置測量值為第一幀下的相機歸一化平面坐標 232 edge->setParameterId(0,0); //最后一位設置使用的相機參數(因為上面僅僅輸入了一個相機參數id=0, 對應上面camer->setId(0),第一個參數0不知道是什么，但是必須為0) 233 edge->setInformation ( Eigen::Matrix2d::Identity() ); //信息矩陣2x2的單位陣 234 optimizer.addEdge( edge ); 235 } 236 //第一幀路標點鏈接到第二幀位姿節點 237 for( int i=0 ;i < points1_2d.size() ; i++){ 238 auto edge = new g2o::EdgeProjectXYZ2UV(); //設置鏈接到第二幀的邊 239 edge->setVertex( 0, dynamic_cast< g2o::VertexSBAPointXYZ * >( optimizer.vertex( landmarkVertexIndex + i) ) ); //第一幀坐標系下的路標點 240 edge->setVertex( 1, dynamic_cast< g2o::VertexSE3Expmap *> ( optimizer.vertex(1) ) ); //連接到第二個位姿節點 241 edge->setMeasurement( Eigen::Vector2d ( points2_2d[i].x, points2_2d[i].y ) ); //設置測量值為第二幀下的相機歸一化平面坐標 242 edge->setInformation( Eigen::Matrix2d::Identity() ); //信息矩陣為2x2 實際上就是誤差的加權為1：1的 243 edge->setParameterId(0,0); 244 optimizer.addEdge( edge ); 245 } 246 247 //run 算法 248 cout<<"開始優化!"<<endl; 249 chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); 250 optimizer.setVerbose ( true ); //設置詳細信息 251 optimizer.initializeOptimization( ); //優化器初始化 252 optimizer.optimize( 100 ); //進行最多100次的優化 253 chrono::steady_clock::time_point t2 = chrono::steady_clock::now(); 254 cout<<"優化結束!"<<endl; 255 chrono::duration< double > time_used = chrono::duration_cast< chrono::duration<double> >( t2 -t1 ); 256 cout<<"optimization costs time: "<<time_used.count() << "seconds."<<endl; 257 cout<<endl<<"after optimization:"<<endl; 258 //輸出優化節點的位姿 estimate()輸出的是SE3類型 Eigen::Isometry3d 實際上就是4x4的一個矩陣表示變換矩陣 這個類初始化可以是李代數 259 //這里有一點不明白的是 Eigen::Isometry3d()為什么可以用estimate()返回的SE3Quat類型初始化？？？？？？？ 260 cout<<"T="<<endl<<Eigen::Isometry3d ( dynamic_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(1))->estimate()).matrix() <<endl; 261 /* g2o::SE3Quat a(); 262 Eigen::Isometry3d( a);*/ 263 } 264 #else 265 void bundleAdjustment ( 266 const vector< Point3f > &points_3d, 267 const vector< Point2f > &points_2d,//這里加不加引用 都是會報錯 268 const Mat& K, 269 Mat& R, Mat& t ) 270 { 271 // 初始化g2o 272 typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block; // pose 維度為 6, landmark 維度為 3 273 std::unique_ptr<Block::LinearSolverType> linearSolver( new g2o::LinearSolverCSparse<Block::PoseMatrixType>() ); 274 std::unique_ptr<Block> solver_ptr ( new Block ( std::move(linearSolver) ) ); 275 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr) ); 276 /* Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 線性方程求解器 277 Block* solver_ptr = new Block ( std::unique_ptr<Block::LinearSolverType>(linearSolver) ); // 矩陣塊求解器 278 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::unique_ptr<g2o::Solver>(solver_ptr) );*/ 279 g2o::SparseOptimizer optimizer; 280 optimizer.setAlgorithm ( solver ); 281 282 // vertex 283 g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose 284 Eigen::Matrix3d R_mat; 285 R_mat << 286 R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ), 287 R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ), 288 R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 ); 289 pose->setId ( 0 ); 290 //設置頂點估計值 然后才會用BA再次優化 291 pose->setEstimate ( g2o::SE3Quat ( 292 R_mat, 293 Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) ) 294 ) ); 295 optimizer.addVertex ( pose ); 296 297 int index = 1; 298 for ( const Point3f &p:points_3d ) // landmarks 在執行這里的時候，實際上是所有空間點（匹配好的）組成的頂點 299 { 300 g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ(); 301 point->setId ( index++ ); 302 point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) ); 303 point->setMarginalized ( true ); // g2o 中必須設置 marg 參見第十講內容 待注釋？？ 304 optimizer.addVertex ( point ); 305 } 306 307 // parameter: camera intrinsics 308 g2o::CameraParameters* camera = new g2o::CameraParameters ( 309 K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0 310 ); 311 camera->setId ( 0 ); 312 optimizer.addParameter ( camera ); 313 314 // edges 315 index = 1; 316 for ( const Point2f &p:points_2d )//每個點都會與位姿節點鏈接 所以就有那么多的邊 317 { 318 g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV(); 319 edge->setId ( index ); 320 //下行轉化 要用動態類型轉換 321 //將2元邊連接上頂點 322 edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );//空間點類型指針 323 edge->setVertex ( 1, pose ); //位姿類型指針 324 edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) ); //設置測量值 325 edge->setParameterId ( 0,0 ); 326 edge->setInformation ( Eigen::Matrix2d::Identity() ); //因為誤差向量為2維，所以信息矩陣也是2維，這里設置加權為1 即單位陣 327 optimizer.addEdge ( edge ); 328 index++; 329 } 330 331 chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); 332 optimizer.setVerbose ( true ); 333 optimizer.initializeOptimization(); 334 optimizer.optimize ( 100 ); 335 chrono::steady_clock::time_point t2 = chrono::steady_clock::now(); 336 chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 ); 337 cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl; 338 339 cout<<endl<<"after optimization:"<<endl; 340 cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl; 341 } 342 #endif
參考資料:
視覺slam十四講從理論到實踐
歡迎大家關注我的微信公眾號「佛系師兄」，里面有關於 Ceres 以及 OpenCV 等更多技術文章。
比如
「反復研究好幾遍，我才發現關於 CMake 變量還可以這樣理解！」
更多好的文章會優先在里面不定期分享！打開微信客戶端，掃描下方二維碼即可關注！
