本资源为python和C++联合编程的示例vs工程源码，为g2o的ba优化程序 供参考

#include <pybind11/pybind11.h> #include <pybind11/numpy.h> namespace py = pybind11; // for opencv #include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp> #include <opencv2/features2d/features2d.hpp> // for g2o #include <g2o/core/sparse_optimizer.h> #include <g2o/core/block_solver.h> #include <g2o/core/robust_kernel.h> #include <g2o/core/robust_kernel_impl.h> #include <g2o/core/optimization_algorithm_levenberg.h> #include <g2o/solvers/cholmod/linear_solver_cholmod.h> #include <g2o/types/slam3d/se3quat.h> #include <g2o/types/sba/types_six_dof_expmap.h> //for eigen #include <Eigen/Core> #include <Eigen/Geometry> using namespace std; int findCorrespondingPoints(const cv::Mat& img1, const cv::Mat& img2, vector<cv::Point2f>& points1, vector<cv::Point2f>& points2) { cv::Ptr<cv::FeatureDetector> orb = cv::ORB::create(); //cv::ORB orb; vector<cv::KeyPoint> kp1, kp2; cv::Mat desp1, desp2; orb->detectAndCompute(img1, cv::Mat(), kp1, desp1); orb->detectAndCompute(img2, cv::Mat(), kp2, desp2); cout << "分别找到了" << kp1.size() << "和" << kp2.size() << "个特征点" << endl; cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming"); double knn_match_ratio = 0.8; vector< vector<cv::DMatch> > matches_knn; matcher->knnMatch(desp1, desp2, matches_knn, 2); // 输出匹配点 cv::Mat img_matches; cv::drawMatches(img1, kp1, img2, kp2, matches_knn, img_matches); cv::imshow("Matches", img_matches); cv::waitKey(0); vector< cv::DMatch > matches; for (size_t i = 0; i < matches_knn.size(); i++) { if (matches_knn[i][0].distance < knn_match_ratio * matches_knn[i][1].distance) matches.push_back(matches_knn[i][0]); } if (matches.size() <= 20) //匹配点太少 return false; for (auto m : matches) { points1.push_back(kp1[m.queryIdx].pt); points2.push_back(kp2[m.trainIdx].pt); } return true; } // 寻找两个图像中的对应点，像素坐标系 // 输入：img1, img2 两张图像 // 输出：points1, points2, 两组对应的2D点 // 将Python中的图像传递给C++进行处理 py::array_t<double> ab(py::array_t<uint8_t> &imge1, py::array_t<uint8_t> &imge2){ //创建全0 numpy数组 py::array_t<double> output_array({3,4}); auto buf = output_array.mutable_data(); for (int i = 0; i < 3; i++) { for (int j = 0; j < 4; j++) { buf[i * 4 + j] = 0; } } // 将输入图像转换为OpenCV的Mat对象 auto bufimg1 = imge1.request(); auto bufimg2 = imge2.request(); int type = CV_8UC3; int channels = 3; if (bufimg1.ndim == 3) { channels = bufimg1.shape[2]; if (channels == 4) { type = CV_8UC4; } } //如果不是3或4通道则退出 if (channels != 3 && channels != 4) { return output_array; } cv::Mat img1(bufimg1.shape[0], bufimg1.shape[1], type, bufimg1.ptr); cv::Mat img2(bufimg2.shape[0], bufimg2.shape[1], type, bufimg2.ptr); // 相机内参 double cx = 325.5; double cy = 253.5; double fx = 518.0; double fy = 519.0; // 调用格式：命令 [第一个图] [第二个图] //std::cout << "输入图像路径" << a << "和" << b << std::endl; //img1 = cv::imread(a); //img2 = cv::imread(b); // 找到对应点 std::vector<cv::Point2f> pts1, pts2; if (findCorrespondingPoints(img1, img2, pts1, pts2) == false) { std::cout << "匹配点不够！" << std::endl; return output_array; } std::cout << "找到了" << pts1.size() << "组对应特征点。" << std::endl; // 构造g2o中的图 /* */ // 先构造求解器 g2o::SparseOptimizer optimizer; // 6*3 的参数 typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> Block; // 使用Cholmod中的线性方程求解器 typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType; typedef g2o::LinearSolverCholmod<BlockSolverType::PoseMatrixType> LinearSolverType; auto solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>())); optimizer.setAlgorithm(solver); optimizer.setVerbose(false); /* * //g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>(); Block::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<Block::PoseMatrixType>(); Block* block_solver =new Block(linearSolver); // L-M 下降 g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg(block_solver); optimizer.setAlgorithm(algorithm); optimizer.setVerbose(false); */ // 添加节点 // 两个位姿节点 for (int i = 0; i < 2; i++) { g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap(); v->setId(i); if (i == 0) v->setFixed(true); // 第一个点固定为零 // 预设值为单位Pose，因为我们不知道任何信息 v->setEstimate(g2o::SE3Quat()); optimizer.addVertex(v); } // 很多个特征点的节点观测值 // 以第一帧为准 for (size_t i = 0; i < pts1.size(); i++) { g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ(); v->setId(2 + i); // 由于深度不知道，只能把深度设置为1了 double z = 1; double x = (pts1[i].x - cx) * z / fx; double y = (pts1[i].y - cy) * z / fy; /* * v->setMarginalized函数的作用是将节点标记为边缘化节点。 边缘化节点是指在优化过程中，将该节点的估计值从优化变量中剔除， 只保留其对应的误差项。这样做的好处是可以减少优化变量的数量， 从而降低计算复杂度。同时，边缘化节点还可以提高优化的精度和鲁棒性， 因为它可以将一些不确定的变量边缘化掉，从而减少误差的传递。需要注意 的是，只有在节点的所有边都被边缘化后，该节点才能被边缘化。因此，在 使用v->setMarginalized函数时，需要保证该节点的所有边都已经被正确地边缘化了。 */ v->setMarginalized(true); v->setEstimate(Eigen::Vector3d(x, y, z)); optimizer.addVertex(v); } // 准备相机参数 g2o::CameraParameters* camera = new g2o::CameraParameters(fx, Eigen::Vector2d(cx, cy), 0); camera->setId(0); optimizer.addParameter(camera); /* * 使用EdgeProjectXYZ2UV类型，设置边的两个顶点分别为特征点节点和位姿节点，测量值为(pts1[i].x, pts1[i].y) 或(pts2[i].x, pts2[i].y)， 信息矩阵为单位矩阵，参数id为0，核函数为Huber核函数，并将其添加到优化器中。 */ // 准备边 // 第一帧 vector<g2o::EdgeProjectXYZ2UV*> edges; for (size_t i = 0; i < pts1.size(); i++) {//创建一个新的边对象，类型为g2o::EdgeProjectXYZ2UV g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV(); //设置边的第一个顶点，即3D点的顶点。这里的i+2是因为在优化器中，前两个顶点是相机位姿的顶点， //所以3D点的顶点编号从2开始。dynamic_cast是将基类指针转换为派生类指针的方法。 edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i + 2))); //设置边的第二个顶点，即相机位姿的顶点。这里的相机位姿是指将相机从世界坐标系变换到相机坐标系的变换矩阵。 edge->setVertex(1, dynamic_cast<g2o::VertexSE3Expmap*> (optimi