1 /** 2 * BA Example 3 * Author: Xiang Gao 4 * Date: 2016.3 5 * Email: 6 * 7 * 在这个程序中，我们读取两张图像，进行特征匹配。然后根据匹配得到的特征，计算相机运动以及特征点的位置。这是一个典型的Bundle Adjustment，我们用g2o进行优化。 8 */ 9 10 // for std 11 #include <iostream> 12 // for opencv 13 #include <opencv2/core/core.hpp> 14 #include <opencv2/highgui/highgui.hpp> 15 #include <opencv2/features2d/features2d.hpp> 16 #include <boost/concept_check.hpp> 17 // for g2o 18 #include <g2o/core/sparse_optimizer.h> 19 #include <g2o/core/block_solver.h> 20 #include <g2o/core/robust_kernel.h> 21 #include <g2o/core/robust_kernel_impl.h> 22 #include <g2o/core/optimization_algorithm_levenberg.h> 23 #include <g2o/solvers/cholmod/linear_solver_cholmod.h> 24 #include <g2o/types/slam3d/se3quat.h> 25 #include <g2o/types/sba/types_six_dof_expmap.h> 26 27 28 using namespace std; 29 30 // 寻找两个图像中的对应点，像素坐标系 31 // 输入：img1, img2 两张图像 32 // 输出：points1, points2, 两组对应的2D点 33 int findCorrespondingPoints( const cv::Mat& img1, const cv::Mat& img2, vector<cv::Point2f>& points1, vector<cv::Point2f>& points2 ); 34 35 // 相机内参 36 double cx = 325.5; 37 double cy = 253.5; 38 double fx = 518.0; 39 double fy = 519.0; 40 41 int main( int argc, char argv ) 42 { 43 // 调用格式：命令 [第一个图] [第二个图] 44 if (argc != 3) 45 { 46 cout<<"Usage: ba_example img1, img2"<<endl; 47 exit(1); 48 } 49 50 // 读取图像 51 cv::Mat img1 = cv::imread( argv[1] ); 52 cv::Mat img2 = cv::imread( argv[2] ); 53 54 // 找到对应点 55 vector<cv::Point2f> pts1, pts2; 56 if ( findCorrespondingPoints( img1, img2, pts1, pts2 ) == false ) 57 { 58 cout<<"匹配点不够！"<<endl; 59 return 0; 60 } 61 cout<<"找到了"<<pts1.size()<<"组对应特征点。"<<endl; 62 // 构造g2o中的图 63 // 先构造求解器 64 g2o::SparseOptimizer optimizer; 65 // 使用Cholmod中的线性方程求解器 66 g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> (); 67 // 6*3 的参数 68 g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3( linearSolver ); 69 // L-M 下降 70 g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg( block_solver ); 71 72 optimizer.setAlgorithm( algorithm ); 73 optimizer.setVerbose( false ); 74 75 // 添加节点 76 // 两个位姿节点 77 for ( int i=0; i<2; i++ ) 78 { 79 g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap(); 80 v->setId(i); 81 if ( i == 0) 82 v->setFixed( true ); // 第一个点固定为零 83 // 预设值为单位Pose，因为我们不知道任何信息 84 v->setEstimate( g2o::SE3Quat() ); 85 optimizer.addVertex( v ); 86 } 87 // 很多个特征点的节点 88 // 以第一帧为准 89 for ( size_t i=0; i<pts1.size(); i++ ) 90 { 91 g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ(); 92 v->setId( 2 + i ); 93 // 由于深度不知道，只能把深度设置为1了 94 double z = 1; 95 double x = ( pts1[i].x - cx ) * z / fx; 96 double y = ( pts1[i].y - cy ) * z / fy; 97 v->setMarginalized(true); 98 v->setEstimate( Eigen::Vector3d(x,y,z) ); 99 optimizer.addVertex( v ); 100 } 101 102 // 准备相机参数 103 g2o::CameraParameters* camera = new g2o::CameraParameters( fx, Eigen::Vector2d(cx, cy), 0 ); 104 camera->setId(0); 105 optimizer.addParameter( camera ); 106 107 // 准备边 108 // 第一帧 109 vector<g2o::EdgeProjectXYZ2UV*> edges; 110 for ( size_t i=0; i<pts1.size(); i++ ) 111 { 112 g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV(); 113 edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2)) ); 114 edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*> (optimizer.vertex(0)) ); 115 edge->setMeasurement( Eigen::Vector2d(pts1[i].x, pts1[i].y ) ); 116 edge->setInformation( Eigen::Matrix2d::Identity() ); 117 edge->setParameterId(0, 0); 118 // 核函数 119 edge->setRobustKernel( new g2o::RobustKernelHuber() ); 120 optimizer.addEdge( edge ); 121 edges.push_back(edge); 122 } 123 // 第二帧 124 for ( size_t i=0; i<pts2.size(); i++ ) 125 { 126 g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV(); 127 edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2)) ); 128 edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*> (optimizer.vertex(1)) ); 129 edge->setMeasurement( Eigen::Vector2d(pts2[i].x, pts2[i].y ) ); 130 edge->setInformation( Eigen::Matrix2d::Identity() ); 131 edge->setParameterId(0,0); 132 // 核函数 133 edge->setRobustKernel( new g2o::RobustKernelHuber() ); 134 optimizer.addEdge( edge ); 135 edges.push_back(edge); 136 } 137 138 cout<<"开始优化"<<endl; 139 optimizer.setVerbose(true); 140 optimizer.initializeOptimization(); 141 optimizer.optimize(10); 142 cout<<"优化完毕"<<endl; 143 144 //我们比较关心两帧之间的变换矩阵 145 g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*>( optimizer.vertex(1) ); 146 Eigen::Isometry3d pose = v->estimate(); 147 cout<<"Pose="<<endl<<pose.matrix()<<endl; 148 149 // 以及所有特征点的位置 150 for ( size_t i=0; i<pts1.size(); i++ ) 151 { 152 g2o::VertexSBAPointXYZ* v = dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2)); 153 cout<<"vertex id "<<i+2<<", pos = "; 154 Eigen::Vector3d pos = v->estimate(); 155 cout<<pos(0)<<","<<pos(1)<<","<<pos(2)<<endl; 156 } 157 158 // 估计inlier的个数 159 int inliers = 0; 160 for ( auto e:edges ) 161 { 162 e->computeError(); 163 // chi2 就是 error*Omega*error, 如果这个数很大，说明此边的值与其他边很不相符 164 if ( e->chi2() > 1 ) 165 { 166 cout<<"error = "<<e->chi2()<<endl; 167 } 168 else 169 { 170 inliers++; 171 } 172 } 173 174 cout<<"inliers in total points: "<<inliers<<"/"<<pts1.size()+pts2.size()<<endl; 175 optimizer.save("ba.g2o"); 176 return 0; 177 } 178 179 180 int findCorrespondingPoints( const cv::Mat& img1, const cv::Mat& img2, vector<cv::Point2f>& points1, vector<cv::Point2f>& points2 ) 181 { 182 cv::ORB orb; 183 vector<cv::KeyPoint> kp1, kp2; 184 cv::Mat desp1, desp2; 185 orb( img1, cv::Mat(), kp1, desp1 ); 186 orb( img2, cv::Mat(), kp2, desp2 ); 187 cout<<"分别找到了"<<kp1.size()<<"和"<<kp2.size()<<"个特征点"<<endl; 188 189 cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create( "BruteForce-Hamming"); 190 191 double knn_match_ratio=0.8; 192 vector< vector<cv::DMatch> > matches_knn; 193 matcher->knnMatch( desp1, desp2, matches_knn, 2 ); 194 vector< cv::DMatch > matches; 195 for ( size_t i=0; i<matches_knn.size(); i++ ) 196 { 197 if (matches_knn[i][0].distance < knn_match_ratio * matches_knn[i][1].distance ) 198 matches.push_back( matches_knn[i][0] ); 199 } 200 201 if (matches.size() <= 20) //匹配点太少 202 return false; 203 204 for ( auto m:matches ) 205 { 206 points1.push_back( kp1[m.queryIdx].pt ); 207 points2.push_back( kp2[m.trainIdx].pt ); 208 } 209 210 return true; 211 }
版权声明：
本文来源网络，所有图片文章版权属于原作者，如有侵权，联系删除。
本文网址：https://www.mushiming.com/mjsbk/9816.html