视觉SLAM实践入门——（15）使用g2o求解PnP

使用g2o求解PnP的步骤：

1、定义顶点和边的数据类型

2、提取ORB特征、匹配点对

3、构造顶点、边

4、定义、设置求解器

5、调用求解器的 initializeOptimization 函数求解问题

如果无法链接g2o动态库，可以参考这篇文章的注意事项

https://blog.csdn.net/floatinglong/article/details/116159431

#include <iostream>
using namespace std;#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
using namespace cv;#include <Eigen/Core>
using namespace Eigen;#include <chrono>#include <sophus/se3.hpp>#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>//提取ORB特征并匹配
void findAndMatchOrbFeature(const Mat &img1, const Mat &img2, vector<KeyPoint> &kp1, vector<KeyPoint> &kp2, vector<DMatch> &matches)
{   Ptr<FeatureDetector> detector = ORB::create();Ptr<DescriptorExtractor> desc = ORB::create();Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");//检测角点detector->detect(img1, kp1);detector->detect(img2, kp2);//计算描述子Mat desc1, desc2;    desc->compute(img1, kp1, desc1);desc->compute(img2, kp2, desc2);//匹配点对vector<DMatch> allMatches;matcher->match(desc1, desc2, allMatches);//检测误匹配double minDist = 10000;for(auto m : allMatches)if(m.distance < minDist)  minDist = m.distance;double useDist = max(2*minDist, 30.0);for(auto m : allMatches)   if(m.distance <= useDist)matches.push_back(m);
}// 顶点，模板参数：优化变量维度和数据类型
class Vertex : public g2o::BaseVertex<6, Sophus::SE3d> {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW// 重置virtual void setToOriginImpl() override {    //override保留字表示当前函数重写了基类的虚函数_estimate = Sophus::SE3d();}// 更新virtual void oplusImpl(const double *update) override {Eigen::Matrix<double, 6, 1> updateVector;updateVector << update[0], update[1], update[2], update[3], update[4], update[5]; _estimate = Sophus::SE3d::exp(updateVector) * _estimate;}// 存盘和读盘：留空virtual bool read(istream &in) {}virtual bool write(ostream &out) const {}
};// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class Edge : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, Vertex> {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWEdge(const Eigen::Vector3d &p3d, const Eigen::Matrix3d &k) :  _p3d(p3d), _k(k) {}// 计算误差virtual void computeError() override {        const Vertex *v = static_cast<const Vertex *> (_vertices[0]);    //static_cast将_vertices[0]的类型强制转换为const Edge *const Sophus::SE3d T = v->estimate();    //取出顶点优化变量Eigen::Vector3d tp3d = T * _p3d;  //旋转后的空间坐标tp3d = _k * tp3d / tp3d[2];  //映射到像素平面，前两维是像素坐标，因为这里用到了k的运算，因此k要定义为Eigen库里的类型   Eigen::Vector2d tp2d = tp3d.head<2>();_error = _measurement - tp2d;    //计算误差}// 计算雅可比矩阵
virtual void linearizeOplus() override {const Vertex *v = static_cast<const Vertex *> (_vertices[0]);const Sophus::SE3d T = v->estimate();   //取出上次估计的TEigen::Vector3d tp3d = T * _p3d; //3d点经过旋转double invZ = 1 / tp3d[2], invZ2 = invZ * invZ;double fx = _k(0, 0);double fy = _k(1, 1);double cx = _k(0, 2);double cy = _k(1, 2);_jacobianOplusXi << -fx * invZ,0,fx * tp3d[0] * invZ2,fx * tp3d[0] * tp3d[1] * invZ2,-fx - fx * tp3d[0] * tp3d[0] * invZ2,fx * tp3d[1] * invZ,0,-fy * invZ,fy * tp3d[1] * invZ2,fy + fy * tp3d[1] * tp3d[1] * invZ2,-fy * tp3d[0] * tp3d[1] * invZ2,-fy * tp3d[0] * invZ;}virtual bool read(istream &in) {}virtual bool write(ostream &out) const {}private:Eigen::Vector3d _p3d;Eigen::Matrix3d _k;
};int main(int argc, char **argv)
{if(argc != 4){cout << "error!" << endl;return 0;}Mat img1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);Mat img2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);vector<KeyPoint> kp1, kp2;vector<DMatch> matches;findAndMatchOrbFeature(img1, img2, kp1, kp2, matches);//g2otypedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 2>> BlockSolverType;  //优化变量维度为6，误差维度为2typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型//梯度下降方法auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));g2o::SparseOptimizer optimizer;     // 图模型optimizer.setAlgorithm(solver);   // 设置求解器optimizer.setVerbose(true);       // 打开调试输出//构造顶点Vertex *v = new Vertex();v->setEstimate(Sophus::SE3d());v->setId(0);optimizer.addVertex(v);//取3d-2d点对Eigen::Matrix3d k;k << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1;vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> ps2d;vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> ps3d;    for(auto m : matches){Point2d p2d = kp1[m.queryIdx].pt;int x = p2d.x, y = p2d.y;unsigned short pixel = img3.ptr<unsigned short>(y)[x];if(pixel == 0)    continue;double dPixel = pixel / 5000.0;Point2d pNor2d = pixelToNor(p2d, k);ps3d.push_back(Eigen::Vector3d(pNor2d.x*dPixel, pNor2d.y*dPixel, dPixel));p2d = kp2[m.trainIdx].pt;x = p2d.x;y = p2d.y;ps2d.push_back(Eigen::Vector2d(x, y));}//构造边for (int i = 0; i < (int)ps3d.size(); i++) {Edge *edge = new Edge(ps3d[i], k);edge->setId(i);edge->setVertex(0, v);                // 设置连接的顶点edge->setMeasurement(ps2d[i]);      // 观测值edge->setInformation(Eigen::Matrix2d::Identity()); // 信息矩阵：协方差矩阵之逆，这里没有，填单位阵，维度与误差变量一致optimizer.addEdge(edge);}// 执行优化optimizer.initializeOptimization();optimizer.optimize(10);  //迭代次数// 输出优化值Sophus::SE3d T = v->estimate();cout << "estimated model: " << endl << T.matrix() << endl;return 0;
}

视觉SLAM实践入门——（15）使用g2o求解PnP相关推荐

视觉SLAM实践入门——（3）运动的可视化演示
旋转矩阵等数学表示方式不直观,考虑将相机的运动轨迹画到一个窗口中. 假设轨迹文件已经存储在trajectory.txt,每一行用下面格式存储: time,tx,ty,tz,qx,qy,qz,qw, 其 ...
视觉SLAM实践入门——（12）三角测量
前面通过对极约束估计了相机的 R,t,这一节通过三角测量可以恢复深度值,得到特征点的空间位置(估计值) 利用opencv进行三角测量的步骤: 1.定义旋转矩阵和平移向量组成的增广矩阵 2.计算特征点在 ...
视觉SLAM笔记（15）李群与李代数
视觉SLAM笔记(15) 李群与李代数 1. 前言 2. 基础 3. 群 4. 李代数的引出 5. 李代数的定义 6. 李代数so(3) 7. 李代数se(3) 1. 前言之前介绍了三维世界中刚体运 ...
视觉SLAM笔记（53） g2o 操作后端优化
视觉SLAM笔记(53) g2o 操作后端优化 1. BA 数据集 2. g2o 求解 BA 3. 求解 1. BA 数据集目录下的 common 文件夹是实验的数据集部分它的布局如图所示其中, ...
视觉SLAM笔记（29） g2o
视觉SLAM笔记(29) g2o 1. 图优化 2. g2o 的编译与安装 3. 拟合曲线 1. 图优化图优化是一种将非线性优化与图论结合起来的理论因此在使用它之前,需要花一点篇幅介绍一个图优化理 ...
2d与2.5d坐标转换_视觉SLAM:搞定坐标系、三角测量、PnP
1.正文四个坐标系:世界坐标系.相机坐标系.图像坐标系.像素坐标系. 世界坐标系:机器人或相机运动过程中,肯定需要知道它的位置,因此需要设定世界坐标系,认定固定不动,作为参考坐标系,描述世界中的任 ...
视觉SLAM十四讲 ch3 Ubuntu18.04 KDevelop的使用及Eigen实践入门笔记
视觉SLAM十四讲 ch3 Ubuntu18.04 KDevelop的使用及Eigen实践入门笔记一.创建KDevelop项目二.编写程序一.创建KDevelop项目你的电脑上如果还没有安装 ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-特征点法和特征提取和匹配实践
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
【slam十四讲第二版】【课本例题代码向】【第九讲~后端Ⅰ】【安装Meshlab】【BAL数据集格式】【ceres求解BA】【g2o求解BA】
[slam十四讲第二版][课本例题代码向][第九讲~后端Ⅰ][安装Meshlab][BAL数据集格式][ceres求解BA][g2o求解BA] 0 前言 1 安装Meshlab: 三维几何网格处理 2 ...

视觉SLAM实践入门——（15）使用g2o求解PnP

视觉SLAM实践入门——（15）使用g2o求解PnP相关推荐

最新文章

热门文章