版权声明：本文为博主原创文章，转载请注明出处: 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十四讲从理论到实践
