VSLAM算法（二）：PnP求解 3D-2D 相机位姿及 BA优化算法

BA优化模型如下：优化变量（空间位置P和相机位姿），边（P在相机平面的投影像素坐标）

 // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false );cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵

void bundleAdjustment (const vector< Point3f > points_3d,const vector< Point2f > points_2d,const Mat& K,Mat& R, Mat& t )
{// 初始化g2otypedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3//Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverCSparse<Block::PoseMatrixType>());//Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器//std::unique_ptr<Block> solver_ptr ( new Block ( linearSolver));std::unique_ptr<Block> solver_ptr ( new Block ( std::move(linearSolver)));    // 矩阵块求解器//g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr));g2o::SparseOptimizer optimizer;optimizer.setAlgorithm ( solver );//申明一个位姿优化模型// vertexg2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera poseEigen::Matrix3d R_mat;R_mat <<R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );pose->setId ( 0 );pose->setEstimate ( g2o::SE3Quat (R_mat,Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )) );optimizer.addVertex ( pose );//添加3D路标点int index = 1;for ( const Point3f p:points_3d )   // landmarks{g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();point->setId ( index++ );point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容optimizer.addVertex ( point );}// 添加相机参数 parameter: camera intrinsicsg2o::CameraParameters* camera = new g2o::CameraParameters (K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0);camera->setId ( 0 );optimizer.addParameter ( camera );//添加边 edgesindex = 1;for ( const Point2f p:points_2d ){g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();edge->setId ( index );edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );edge->setVertex ( 1, pose );edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );edge->setParameterId ( 0,0 );edge->setInformation ( Eigen::Matrix2d::Identity() );optimizer.addEdge ( edge );index++;}chrono::steady_clock::time_point t1 = chrono::steady_clock::now();optimizer.setVerbose ( true );optimizer.initializeOptimization();optimizer.optimize ( 100 );chrono::steady_clock::time_point t2 = chrono::steady_clock::now();chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;cout<<endl<<"after optimization:"<<endl;cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
}

PnP 函数原型：

void solvePnP(InputArray objectPoints,InputArray imagePoints,InputArray cameraMatrix, InputArray distCoeffs,OutputArray rvec, OutputArray tvec,bool useExtrinsicGuess=false, int flags = CV_ITERATIVE)
参数解释：
objectPoints：世界坐标系中的3D点坐标，单位mm
imagePoints：图像坐标系中点的坐标，单位像素
cameraMatrix：相机内参矩阵
distCoeffs：畸变系数
rvec：旋转矩阵——》是向量形式，需用Rodrigues转换成旋转矩阵形式
tvec：平移矩阵
useExtrinsicGuess：是否输出平移矩阵和旋转矩阵，默认为false
flags：SOLVEPNP _ITERATIVE、SOLVEPNP _P3P、SOLVEPNP _EPNP、SOLVEPNP _DLS、SOLVEPNP _UPNP
----------------------------------------------------------------------------------------
objectPoints - 世界坐标系下的控制点的坐标，vector<Point3f>的数据类型在这里可以使用
imagePoints - 在图像坐标系下对应的控制点的坐标。vector<Point2f>在这里可以使用
cameraMatrix - 相机的内参矩阵
distCoeffs - 相机的畸变系数
以上两个参数通过相机标定可以得到。相机的内参数的标定参见：http://www.cnblogs.com/star91/p/6012425.html
rvec - 输出的旋转向量。使坐标点从世界坐标系旋转到相机坐标系
tvec - 输出的平移向量。使坐标点从世界坐标系平移到相机坐标系
flags - 默认使用CV_ITERATIV迭代法作者：喻茸sophie
链接：https://www.jianshu.com/p/b97406d8833c

ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
/** 这个m的类型:DMatch* DMatch主要用来储存匹配信息的结构体，query是要匹配的描述子，train是被匹配的描述子；* 在Opencv中进行匹配时* void DescriptorMatcher::match( const Mat& queryDescriptors,const Mat& trainDescriptors, vector<DMatch>& matches, const Mat& mask ) const* match函数的参数中位置在前面的为query descriptor，后面的是 train descriptor* 例如：query descriptor的数目为20，train descriptor数目为30，则DescriptorMatcher::match后的vector<DMatch>的size为20* 若反过来，则vector<DMatch>的size为30*/
struct CV_EXPORTS_W_SIMPLE DMatch
{ //默认构造函数，FLT_MAX是无穷大 //#define FLT_MAX         3.402823466e+38F        /* max value */ CV_WRAP DMatch() : queryIdx(-1), trainIdx(-1), imgIdx(-1), distance(FLT_MAX) {} //DMatch构造函数 CV_WRAP DMatch( int _queryIdx, int _trainIdx, float _distance ) : queryIdx(_queryIdx), trainIdx(_trainIdx), imgIdx(-1), distance(_distance) {} //DMatch构造函数 CV_WRAP DMatch( int _queryIdx, int _trainIdx, int _imgIdx, float _distance ) : queryIdx(_queryIdx), trainIdx(_trainIdx), imgIdx(_imgIdx), distance(_distance) {} //queryIdx为query描述子的索引，match函数中前面的那个描述子 CV_PROP_RW int queryIdx; // query descriptor index //trainIdx为train描述子的索引，match函数中后面的那个描述子 CV_PROP_RW int trainIdx; // train descriptor index //imgIdx为进行匹配图像的索引 //例如已知一幅图像的sift描述子，与其他十幅图像的描述子进行匹配，找最相似的图像，则imgIdx此时就有用了。 CV_PROP_RW int imgIdx;   // train image index //distance为两个描述子之间的距离 CV_PROP_RW float distance; //DMatch比较运算符重载，比较的是DMatch中的distance，小于为true，否则为false // less is better bool operator<( const DMatch &m ) const { return distance < m.distance; }
};
原文：https://blog.csdn.net/robinhjwy/article/details/77801950

VSLAM算法（二）：PnP求解 3D-2D 相机位姿及 BA优化算法相关推荐

《MATLAB智能算法30个案例》：第12章免疫优化算法在物流配送中心选址中的应用
<MATLAB智能算法30个案例>:第12章免疫优化算法在物流配送中心选址中的应用 1. 前言 2. MATLAB 仿真示例 3. 小结 1. 前言 <MATLAB智能算法30个案 ...
【优化求解】基于自适应模拟退火粒子群优化算法求解单目标优化问题matlab代码
1 简介针对PSO算法在求解问题的优化问题中易陷入局部收敛且收敛速度较慢等缺陷,引入一种初始化改进策略,并将模拟退火算法与PSO算法相结合,提出了一种全新的算法.该算法将寻优过程分为两个阶段:为了提 ...
【优化求解】基于缎蓝园丁鸟优化算法 (SBO)求解单目标问题附matlab代码
1 简介 2 部分代码 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ...
【优化求解】基于吉萨金子塔建造优化算法 GPC求解最优目标matlab代码
1 简介吉萨金子塔建造优化算法(Giza Pyramids Construction,GPC)是于2020年提出的一种,新型智能优化算法,其仿照金字塔建造的过程,具有寻优能力强,精度高的特点. No ...
【蝗虫算法】基于混合柯西变异和均匀分布改进蝗虫优化算法求解单目标优化问题matlab代码
1 简介传统蝗虫优化算法在处理优化问题时依然存在收敛速度慢,易陷入局部最优的不足.为此,提出了融合混沌映射和柯西变异机制的非线性蝗虫优化算法CCGOA.通过融合混沌Tent映射与反向学习机制,对种群 ...
差分进化算法_基于差分进化的水泥烧成系统动态优化算法
基于差分进化的水泥烧成系统动态优化算法郝晓辰, 冀亚坤, 郑立召, 史鑫, 赵彦涛燕山大学电气工程学院,河北秦皇岛 066004 [摘要]针对水泥烧成过程的资源浪费以及难以建立有效数学机理模 ...
基于沙猫群优化算法的线性规划求解matlab程序
基于沙猫群优化算法的线性规划求解matlab程序 1 沙猫群优化算法沙猫的中文学名叫沙丘猫,俗名沙漠猫,与荒漠猫名字相似,但却是两种不同的猫科动物.沙猫生活在茫茫沙漠里,主要分布在分布于非洲北部,阿 ...
c++/opencv利用相机位姿估计实现2D图像像素坐标到3D世界坐标的转换
最近在做自动泊车项目中的车位线检测,用到了将图像像素坐标转换为真实世界坐标的过程,该过程可以通过世界坐标到图像像素坐标之间的关系进行求解,在我的一篇博文中已经详细讲解了它们之间的数学关系,不清楚的童鞋 ...
《增强现实：原理、算法与应用》读书笔记（5）运动恢复结构（上）初始化、相机位姿估计、集束调整
<增强现实:原理.算法与应用>读书笔记(5)运动恢复结构(上)初始化.相机位姿估计.集束调整运动恢复结构(SfM)是一种从运动的相机拍摄的图像或视频序列中自动地恢复出相机运动轨迹以及场景 ...
鲸鱼优化算法WOA求解旅行商TSP优化问题（2022.6.2）
鲸鱼优化算法WOA求解旅行商TSP优化问题(2022.6.2) 引言 1.鲸鱼优化算法WOA 1.1 WOA算法原理介绍 1.1.1 包围猎物 1.1.2 气泡网式攻击猎物(开发阶段) 1.1.3 寻 ...

VSLAM算法（二）：PnP求解 3D-2D 相机位姿及 BA优化算法

VSLAM算法（二）：PnP求解 3D-2D 相机位姿及 BA优化算法相关推荐

最新文章

热门文章