目录
- 1、前言
- 2、准备工作
- 安装git
- 安装vcpkg
- （1）下载
- （2）安装
- （3）集成至vs
- 安装cmake
- 3、安装g2o
- 4、安装opencv
- （1）下载
- （2）双击安装
- （3）环境变量和system文件夹设置
- 使用g2o进行BA优化
- 5、总结
1、前言
本篇博客主要介绍如何在Windows下安装g2o，并利用g2o和OpenCV库实现一个两帧之间的ORB特征点检测和BA优化的C++程序。ORB是一种既能检测特征点，又能描述特征点的算法，BA是一种优化算法，可以优化相机位姿和三维点云。本文将详细介绍如何在Windows下安装g2o，并利用g2o和OpenCV库实现一个两帧之间的ORB特征点检测和BA优化的C++程序。首先，我们需要在Windows下安装g2o。g2o是一个用于图优化的C++库，可以用于SLAM、机器人、计算机视觉等领域。其次，我们需要使用OpenCV库来进行ORB特征点检测。最后，我们将使用g2o库来进行BA优化。在本文中，我们将详细介绍如何在Windows下安装g2o，并利用g2o和OpenCV库实现一个两帧之间的ORB特征点检测和BA优化的C++程序。
2、准备工作
安装git
需要安装git工具，可以上官网去下载安装软件。直接选择默认选项安装到底就可以。
安装vcpkg
经过多种方法安装失败后，本人认为，通过vcpkg安装g2o库是最为方便的，vcpkg可以自动安装g2o所需要的依赖库。vcpkg本身的安装也非常方便。
关于安装vcpkg详细步骤如下：
（1）下载
在D盘为 vcpkg 的克隆实例创建目录。
打开cmd，进入创建的目录，从 GitHub 克隆 vcpkg 存储库：https://github.com/Microsoft/vcpkg。
git clone https://github.com/microsoft/vcpkg
或者直接上github去打包下载。
（2）安装
下载完后，解压到安装目录。然后cmd进入vcpkg目录内，里面有个.bat文件，在 vcpkg 根目录下，cmd 下运行 vcpkg 引导程序命令：
bootstrap-vcpkg.bat
就算完成了。
（3）集成至vs
cmd进到vcpkg目录下，只要允许这个命令就可以
.\vcpkg integrate install如果后期不想集成可以通过指令去除
.\vcpkg integrate remove
安装cmake
这个也简单，直接去官网下载安装包，双击安装完事。
先打开下载链接点击下载，下后双击正常安装。
3、安装g2o
非常简单，cmd进入vcpkg目录，输入指令安装：
vcpkg install g2o:x64-windows
耐心等待安装结束，网络要有保障。
4、安装opencv
opencv也可以通过指令安装，也可以去官网下载安装包，我是直接官网下的，弄完需要设置一些东西：
（1）下载
直接从官网下载安装包就行:
（2）双击安装
这个是二进制文件，直接按照到相应的目录下。
（3）环境变量和system文件夹设置
将opencv的\build\x64\vc16\bin文件夹路径写入系统环境变量（path）里面：
同时，需要把bin下面的所有dll文件都放到system32里面：
使用g2o进行BA优化
创建一个vs C++项目，然后直接复制如下代码，就可以编译运行了，对代码进行了一些注释，供参考：
// g2otest.cpp: 定义应用程序的入口点。
//
#pragma once
// 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>/*** BA Example* Author: Xiang Gao* Date: 2016.3* Email: gaoxiang12@mails.tsinghua.edu.cn** 在这个程序中，我们读取两张图像，进行特征匹配。然后根据匹配得到的特征，计算相机运动以及特征点的位置。这是一个典型的Bundle Adjustment，我们用g2o进行优化。*/
using namespace std;// 寻找两个图像中的对应点，像素坐标系
// 输入：img1, img2 两张图像
// 输出：points1, points2, 两组对应的2D点
int findCorrespondingPoints(const cv::Mat& img1, const cv::Mat& img2, vector<cv::Point2f>& points1, vector<cv::Point2f>& points2);// 相机内参，自己标定设定
double cx = 256;
double cy = 256;
double fx = 520;
double fy = 520;int main(int argc, char** argv)
{// 调用格式：命令 [第一个图] [第二个图]cv::Mat img1;cv::Mat img2;if (argc < 3){cout << "无输入图像路径"<< endl;// 读取图像img1 = cv::imread("F:/c++test/g2otest/img/image1.jpg", cv::IMREAD_GRAYSCALE);img2 = cv::imread("F:/c++test/g2otest/img/image2.jpg", cv::IMREAD_GRAYSCALE);}else {cout << "输入图像路径" << argv[1]<<"和" << argv[2] << endl;img1 = cv::imread(argv[1]);img2 = cv::imread(argv[2]);}// 找到对应点vector<cv::Point2f> pts1, pts2;if (findCorrespondingPoints(img1, img2, pts1, pts2) == false){cout << "匹配点不够！" << endl;return 0;}cout << "找到了" << pts1.size() << "组对应特征点。" << 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);// 添加节点// 两个位姿节点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::EdgeProjectXYZ2UVg2o::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*> (optimizer.vertex(0)));//设置边的观测值，即特征点在图像上的坐标。edge->setMeasurement(Eigen::Vector2d(pts1[i].x, pts1[i].y));//设置边的信息矩阵，这里是单位矩阵。edge->setInformation(Eigen::Matrix2d::Identity());//设置边的参数，这里是指定边的参数块为0号参数块。edge->setParameterId(0, 0);// 核函数，设置边的核函数，这里是Huber核函数，用于鲁棒优化。edge->setRobustKernel(new g2o::RobustKernelHuber());//将边添加到优化器中。optimizer.addEdge(edge);//将边对象指针添加到一个vector中，方便后续的操作。edges.push_back(edge);}// 第二帧for (size_t i = 0; i < pts2.size(); i++){g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i + 2)));edge->setVertex(1, dynamic_cast<g2o::VertexSE3Expmap*> (optimizer.vertex(1)));edge->setMeasurement(Eigen::Vector2d(pts2[i].x, pts2[i].y));edge->setInformation(Eigen::Matrix2d::Identity());edge->setParameterId(0, 0);// 核函数edge->setRobustKernel(new g2o::RobustKernelHuber());optimizer.addEdge(edge);edges.push_back(edge);}cout << "开始优化" << endl;optimizer.setVerbose(true);optimizer.initializeOptimization();optimizer.optimize(10);cout << "优化完毕" << endl;//我们比较关心两帧之间的变换矩阵/** 从g2o优化器中获取id为1的VertexSE3Expmap类型的顶点v，并获取其位姿估计值pose，最后输出位姿矩阵。其中，g2o是一个用于非线性优化的C++库，VertexSE3Expmap是g2o中的一个顶点类型，表示一个带有平移和旋转的位姿，estimate()函数返回该顶点的位姿估计值，matrix()函数返回该位姿的变换矩阵。dynamic_cast是C++中的一种类型转换方式，用于将基类指针或引用转换为派生类指针或引用，这里将optimizer.vertex(1)返回的基类指针转换为VertexSE3Expmap类型的指针。*/g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(1));Eigen::Isometry3d pose = v->estimate();cout << "Pose=" << endl << pose.matrix() << endl;// 以及所有特征点的位置for (size_t i = 0; i < pts1.size(); i++){g2o::VertexSBAPointXYZ* v = dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i + 2));cout << "vertex id " << i + 2 << ", pos = ";Eigen::Vector3d pos = v->estimate();cout << pos(0) << "," << pos(1) << "," << pos(2) << endl;}// 估计inlier的个数int inliers = 0;for (auto e : edges){e->computeError();// chi2 就是 error*\Omega*error, 如果这个数很大，说明此边的值与其他边很不相符if (e->chi2() > 1){cout << "error = " << e->chi2() << endl;}else{inliers++;}}cout << "inliers in total points: " << inliers << "/" << pts1.size() + pts2.size() << endl;optimizer.save("ba.g2o");return 0;
}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;
}
5、总结
由于python的性能及机器人导航算法开源工具主流均采用C++，后续要做的工作就是进行c++和python的联合开发，将c++的优秀工具集成进python，这样就可以优势互补，形成一个可行的工程应用解决方案。
