要使用g2o求解PnP，请执行以下操作：
1、定义顶点和边的数据类型
2、提取ORB特征、匹配点对
3、做顶点、边
4、定义、设置求解器
5、调用求解器初始化优化函数求解问题
如果无法链接到g2o动态库，请参见本文中的注意事项
3359 blog.csdn.net/floating long/article/details/116159431
# includeiostreamusingnamespacestd； # include opencv2/core/core.HPP # include opencv2/features 2d/features 2d.HPP # include opencv2/highgui/highgui # include chrono # includesophus/se3.HPP # includeg 2o/core/base _ vertex.h # includeg 2o/core/base _ unary _ ed 提取solver.h # includeg 2o/core/optimization _ algorithm _ GAA ation linear _ solver _ dense.h//orb特征vectorKeyPoint kp1，vector vectordmatchmatches (ptrfeaturedetectordetector=orb : create )； ptrdescriptorextractordesc=orb : create (； prdescriptormatchermatcher=descriptor matcher : create (brute force-hamming )； //检测转角点检测器(img 1，kp1 )； 检测器-检测(img 2、kp2 )； //计算描述符Mat desc1，desc2； DSC-compute(img1，kp1，desc1 )； DSC-compute(img2、kp2、desc2 )； //匹配点对vectorDMatch allMatches； matcher-match(desC1，desc2，allMatches )； //误匹配检测双精度=10000； 自动：所有匹配(for ) if ) m.distancemindist ) minDist=m.distance； doubleusedist=max(2*mindist，30.0 )； 自动：所有匹配(if ) m.distance=used ist (for ) matches.push_back(m ) m； //顶点、模板参数：优化变量维和数据类型class vertex 3360 publi cg2o :3360 base vertex 6，实现soph us :3360 se3d { public : eigen _ man virtualvoidsettoriginimpl () override /复位//virtualvoidoplusimpl (const double * update ) override ) eigen :3603360 maton updateVector update[0]、update[1]、update[2]、update[3]、update[4]和update[5]； _ estimate=soph us : se3d : exp (更新向量(* _ estimate； (//存储和读取)空白{ }虚拟卷读(istream in ) )虚拟卷
ol write(ostream &out) const {}}; // 误差模型 模板参数：观测值维度，类型，连接顶点类型class Edge : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, Vertex> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW Edge(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;}