利用G2o实现BA优化—学习笔记
使用G2o需要图论的知识。一般来说，要优化的参数就是顶点，边就是顶点之间的映射关系。
BA优化，即Bundle Adjustment，需要对相机姿态和特征点同时进行优化，以求得到最优的状态。
对于优化的参数：
x
=
[
ξ
1
ξ
2
.
.
.
ξ
m
∣
p
1
p
2
.
.
.
p
n
]
T
\pmb x=[\xi_1 \quad \xi_2 ...\xi_m| \quad p_1 \quad p_2...p_n]^T
xxx=[ξ1ξ2...ξm∣p1p2...pn]T
我们需要利用到g2o的类来分别定义（当然我们可以自己定义），并定义相关的边，最后利用优化器优化。
官方例程：
// g2o - General Graph Optimization
// Copyright (C) 2012 R. Kümmerle
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <Eigen/Core>
#include <iostream>
#include "g2o/stuff/sampler.h"
#include "g2o/stuff/command_args.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_unary_edge.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include <memory>
#include <boost/shared_array.hpp>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
using namespace std;
/**
* \brief the params, a, b, and lambda for a * exp(-lambda * t) + b
*/
class VertexParams : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexParams()
{
}
virtual bool read(std::istream& /*is*/)
{
cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
return false;
}
virtual bool write(std::ostream& /*os*/) const
{
cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
return false;
}
virtual void setToOriginImpl()
{
cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
}
virtual void oplusImpl(const double* update)
{
Eigen::Vector3d::ConstMapType v(update);
_estimate += v;
}
};
/**
* \brief measurement for a point on the curve
*
* Here the measurement is the point which is lies on the curve.
* The error function computes the difference between the curve
* and the point.
*/
class EdgePointOnCurve : public g2o::BaseUnaryEdge<1, Eigen::Vector2d, VertexParams>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgePointOnCurve()
{
}
virtual bool read(std::istream& /*is*/)
{
cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
return false;
}
virtual bool write(std::ostream& /*os*/) const
{
cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
return false;
}
void computeError()
{
const VertexParams* params = static_cast<const VertexParams*>(vertex(0));
const double& a = params->estimate()(0);
const double& b = params->estimate()(1);
const double& lambda = params->estimate()(2);
double fval = a * exp(-lambda * measurement()(0)) + b;
_error(0) = fval - measurement()(1);
}
};
int main(int argc, char** argv)
{
int numPoints;
int maxIterations;
bool verbose;
std::vector<int> gaugeList;
string dumpFilename;
g2o::CommandArgs arg;
arg.param("dump", dumpFilename, "", "dump the points into a file");
arg.param("numPoints", numPoints, 50, "number of points sampled from the curve");
arg.param("i", maxIterations, 10, "perform n iterations");
arg.param("v", verbose, true, "verbose output of the optimization process");
arg.parseArgs(argc, argv);
// generate random data
double a = 2.;
double b = 0.4;
double lambda = 0.2;
//std::unique_ptr<Eigen::Vector2d[]> points(new Eigen::Vector2d[numPoints]);
boost::shared_array<Eigen::Vector2d> points(new Eigen::Vector2d[numPoints]);
for (int i = 0; i < numPoints; ++i) {
double x = g2o::Sampler::uniformRand(0, 10);
double y = a * exp(-lambda * x) + b;
// add Gaussian noise
y += g2o::Sampler::gaussRand(0, 0.02);
points[i].x() = x;
points[i].y() = y;
}
if (dumpFilename.size() > 0) {
ofstream fout(dumpFilename.c_str());
for (int i = 0; i < numPoints; ++i)
fout << points[i].transpose() << endl;
}
// some handy typedefs
typedef g2o::BlockSolver< g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> > MyBlockSolver;
typedef g2o::LinearSolverDense<MyBlockSolver::PoseMatrixType> MyLinearSolver;
// setup the solver
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(true);
MyLinearSolver* linearSolver = new MyLinearSolver();
MyBlockSolver* solver_ptr = new MyBlockSolver(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
// build the optimization problem given the points
// 1. add the parameter vertex
VertexParams* params = new VertexParams();
params->setId(0);
params->setEstimate(Eigen::Vector3d(1,1,1)); // some initial value for the params
optimizer.addVertex(params);
// 2. add the points we measured to be on the curve
for (int i = 0; i < numPoints; ++i) {
EdgePointOnCurve* e = new EdgePointOnCurve;
e->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
e->setVertex(0, params);
e->setMeasurement(points[i]);
optimizer.addEdge(e);
}
// perform the optimization
optimizer.initializeOptimization();
optimizer.setVerbose(verbose);
optimizer.optimize(maxIterations);
if (verbose)
cout << endl;
// print out the result
cout << "Target curve" << endl;
cout << "a * exp(-lambda * x) + b" << endl;
cout << "Iterative least squares solution" << endl;
cout << "a = " << params->estimate()(0) << endl;
cout << "b = " << params->estimate()(1) << endl;
cout << "lambda = " << params->estimate()(2) << endl;
cout << endl;
// clean up
//delete[] points;
return 0;
}
高翔在书中给出的程序：
#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
using namespace std;
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl() // 重置
{
_estimate << 0,0,0;
}
virtual void oplusImpl( const double* update ) // 更新
{
_estimate += Eigen::Vector3d(update);
}
// 存盘和读盘：留空
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
};
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
// 计算曲线模型误差
void computeError()
{
const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
_error(0,0) = measurement()- std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
}
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
public:
double _x; // x 值， y 值为 _measurement
};
int main( int argc, char** argv )
{
double a=1.0, b=2.0, c=1.0; // 真实参数值
int N=100; // 数据点
double w_sigma=1.0; // 噪声Sigma值
cv::RNG rng; // OpenCV随机数产生器
double abc[3] = {0,0,0}; // abc参数的估计值
vector<double> x_data, y_data; // 数据
cout<<"generating data: "<<endl;
for ( int i=0; i<N; i++ )
{
double x = i/100.0;
x_data.push_back ( x );
y_data.push_back (
exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
);
cout<<x_data[i]<<" "<<y_data[i]<<endl;
}
// 构建图优化，先设定g2o
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每个误差项优化变量维度为3，误差值维度为1
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
Block* solver_ptr = new Block( linearSolver ); // 矩阵块求解器
// 梯度下降方法，从GN, LM, DogLeg 中选
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm( solver ); // 设置求解器
optimizer.setVerbose( true ); // 打开调试输出
// 往图中增加顶点
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
// 往图中增加边
for ( int i=0; i<N; i++ )
{
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
edge->setId(i);
edge->setVertex( 0, v ); // 设置连接的顶点
edge->setMeasurement( y_data[i] ); // 观测数值
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
optimizer.addEdge( edge );
}
// 执行优化
cout<<"start optimization"<<endl;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
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<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
// 输出优化值
Eigen::Vector3d abc_estimate = v->estimate();
cout<<"estimated model: "<<abc_estimate.transpose()<<endl;
return 0;
}
基本方法：
g2o::VertexSE3Expmap定义相机的位姿顶点；
g2o::VertexSBAPointXYZ定义特征点的空间向量；g2o::EdgeProjectXYZ2UV 定义边；
优化器中添加顶点和边；
设置相机参数；
优化。
//构造求解器
g2o::SparseOptimizer optimizer;
//线性方程求解器
//g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
//矩阵块求解器
g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3(linearSolver);
//L-M优化算法
g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg(block_solver);
optimizer.setAlgorithm(algorithm);
for(int i = 0;i<num;i++)
{
g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap;
v->setId(i);
v->setFixed(i==0);
v->setEstimate(g2o::SE3Quat());
optimizer.addVertex(v);
}
//添加特征点顶点
for(int i=0;i<points_1.size();i++)
{
g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
Point2d cam = pixel2cam(points_1[i],K);
v->setId(i+num); //已经添加过num个位姿的顶点了
v->setEstimate(Eigen::Vector3d(cam.x,cam.y,1));
v->setMarginalized(true);//把矩阵块分成两个部分，分别求解微量
optimizer.addVertex(v);
}
添加的参数示意：
x
=
[
ξ
1
ξ
2
.
.
.
ξ
m
∣
p
1
p
2
.
.
.
p
n
]
T
\pmb x=[\xi_1 \quad \xi_2 ...\xi_m| \quad p_1 \quad p_2...p_n]^T
xxx=[ξ1ξ2...ξm∣p1p2...pn]T
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(0)));
edge->setMeasurement(Eigen::Vector2d(points_1[i].x,points_1[i].y));
edge->setRobustKernel(new g2o::RobustKernelHuber());
edge->setInformation(Eigen::Matrix2d::Identity());
edge->setParameterId(0,0);
optimizer.addEdge(edge);
optimizer.setVerbose(true);
optimizer.initializeOptimization();
optimizer.optimize(20);
g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*>( optimizer.vertex(1) );
Eigen::Isometry3d pose = v->estimate();
cout<<"Pose="<<endl<<pose.matrix()<<endl;
optimizer.save("ba.g2o");
main.cpp源码：
#include <iostream>
#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/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/types/slam3d/se3quat.h>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/cudafeatures2d.hpp>
#include <opencv2/cudaimgproc.hpp>
#include <Eigen/Core>
#include <vector>
#include <chrono>
using namespace std;
using namespace cv;
void find_feature_matches_GPU(const Mat &img_1, const Mat &img_2,
std::vector<KeyPoint> &keypoints_1,
std::vector<KeyPoint> &keypoints_2,
std::vector<DMatch> &matches);
void find_feature_matches_CPU(const Mat &img_1, const Mat &img_2,
std::vector<KeyPoint> &keypoints_1,
std::vector<KeyPoint> &keypoints_2,
std::vector<DMatch> &matches);
void BASolver(vector<Point2f> &points_1,vector<Point2f> &points_2,Mat &K);
//相机内参
Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
Point2d pixel2cam(const Point2d &p, const Mat &K)
{
return Point2d
(
(p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
);
}
int main(int argc,char**argv)
{
std::string path1 = argv[1];
std::string path2 = argv[2];
if(argc !=3)
{
cout<<"输入参数方法：**/testOptimizerG2o ./1.png ./2.png\n";
return 0;
}
Mat img_1 = imread(path1);
Mat img_2 = imread(path2);
vector<KeyPoint> keypoints_1, keypoints_2;
vector<Point2f> points_1,points_2;
vector<DMatch> matches;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
//use GPU
//find_feature_matches_GPU(img_1,img_2,keypoints_1,keypoints_2,matches);
//use cpu
find_feature_matches_CPU(img_1,img_2,keypoints_1,keypoints_2,matches);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> delay_time = chrono::duration_cast<chrono::duration<double>>(t2 - t1); //milliseconds 毫秒
cout<<"匹配耗时:"<<delay_time.count()<<"秒"<<endl;
for(auto m:matches)
{
points_1.push_back(keypoints_1[m.queryIdx].pt);
points_2.push_back(keypoints_2[m.trainIdx].pt);
}
cout<<"特征点的数量："<<points_1.size()<<" "<<points_2.size()<<endl;
//BA优化
BASolver(points_1,points_2,K);
Mat img_keypoints;
drawMatches(img_1,keypoints_1,img_2,keypoints_2,matches,img_keypoints);
imshow("matches",img_keypoints);
waitKey(0);
return 0;
}
void find_feature_matches_GPU(const Mat &img_1, const Mat &img_2,
std::vector<KeyPoint> &keypoints_1,
std::vector<KeyPoint> &keypoints_2,
std::vector<DMatch> &matches) {
cuda::Stream myStream;
// 利用GPU加速提取特征点
cuda::GpuMat d_img1,d_img2;
d_img1.upload(img_1,myStream);
d_img2.upload(img_2,myStream);
cuda::GpuMat d_img1Gray,d_img2Gray;
cuda::cvtColor(d_img1,d_img1Gray,COLOR_BGR2GRAY,0,myStream);
cuda::cvtColor(d_img2,d_img2Gray,COLOR_BGR2GRAY,0,myStream);
//-- 初始化
cuda::GpuMat d_keypoints1, d_keypoints2;
cuda::GpuMat descriptors_1, descriptors_2,descriptors_1_32F,descriptors_2_32F;
//创建ORB检测
Ptr<cuda::ORB> detector_ORB = cuda::ORB::create();
//-- 第一步:检测 Oriented FAST 角点位置 计算 BRIEF 描述子
detector_ORB->detectAndComputeAsync(d_img1Gray,cuda::GpuMat(),d_keypoints1,descriptors_1,false,myStream);
detector_ORB->convert(d_keypoints1,keypoints_1);
descriptors_1.convertTo(descriptors_1_32F,CV_32F);
detector_ORB->detectAndComputeAsync(d_img2Gray,cuda::GpuMat(),d_keypoints2,descriptors_2,false,myStream);
detector_ORB->convert(d_keypoints2,keypoints_2);
descriptors_2.convertTo(descriptors_2_32F,CV_32F);
//-- 第二步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
//*******************************************利用Match方法进行匹配************************************/
//可取消注释选择此方法
/*
vector<DMatch> match;
Ptr<cuda::DescriptorMatcher> matcher = cuda::DescriptorMatcher::createBFMatcher(NORM_L2);
// BFMatcher matcher ( NORM_HAMMING );
matcher->match(descriptors_1_32F, descriptors_2_32F, match);
//-- 第三步:匹配点对筛选
double min_dist = 1000, max_dist = 0;
for (int i = 0; i < descriptors_1.rows; i++) {
double dist = match[i].distance;
if (dist < min_dist) min_dist = dist;
if (dist > max_dist) max_dist = dist;
}
cout<<"-- Max dist : "<< max_dist<<endl;
cout<<"-- Min dist : "<< min_dist<<endl;
for (int i = 0; i < descriptors_1.rows; i++)
{
if (match[i].distance <= 0.7*max_dist)
{
matches.push_back(match[i]);
}
}
*/
//*******************************************利用KnnMatch方法进行匹配（效果较好）**************************/
vector< vector<DMatch>> knnmatch;
Ptr<cuda::DescriptorMatcher> matcher = cuda::DescriptorMatcher::createBFMatcher(NORM_L2);
// BFMatcher matcher ( NORM_HAMMING );
matcher->knnMatch(descriptors_1_32F, descriptors_2_32F, knnmatch,2);
//-- 第三步:匹配点对筛选
for (int i = 0; i < knnmatch.size(); i++)
{
if (knnmatch[i][0].distance <= 0.8*knnmatch[i][1].distance)
{
matches.push_back(knnmatch[i][0]);
}
}
}
void find_feature_matches_CPU(const Mat &img_1, const Mat &img_2,
std::vector<KeyPoint> &keypoints_1,
std::vector<KeyPoint> &keypoints_2,
std::vector<DMatch> &matches) {
//-- 初始化
Mat descriptors_1, descriptors_2;
Mat imgGray_1,imgGray_2;
cvtColor(img_1,imgGray_1,COLOR_BGR2GRAY);
cvtColor(img_2,imgGray_2,COLOR_BGR2GRAY);
//创建ORB检测
Ptr<ORB> detector_ORB = ORB::create();
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
//-- 第一步:检测 Oriented FAST 角点位置 计算 BRIEF 描述子
detector_ORB->detectAndCompute(imgGray_1,Mat(),keypoints_1,descriptors_1);
detector_ORB->detectAndCompute(imgGray_2,Mat(),keypoints_2,descriptors_2);
//-- 第二步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
vector<DMatch> match;
// BFMatcher matcher ( NORM_HAMMING );
matcher->match(descriptors_1, descriptors_2, match);
//-- 第三步:匹配点对筛选
double min_dist = 1000, max_dist = 0;
for (int i = 0; i < descriptors_1.rows; i++)
{
double dist = match[i].distance;
if (dist < min_dist) min_dist = dist;
if (dist > max_dist) max_dist = dist;
}
cout<<"-- Max dist : "<< max_dist<<endl;
cout<<"-- Min dist : "<< min_dist<<endl;
for (int i = 0; i < descriptors_1.rows; i++)
{
if (match[i].distance <= 0.4*max_dist)
{
matches.push_back(match[i]);
}
}
}
void BASolver(vector<Point2f> &points_1,vector<Point2f> &points_2,Mat &K)
{
//****************************BA优化过程*********************
//构造求解器
g2o::SparseOptimizer optimizer;
//线性方程求解器
//g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
//矩阵块求解器
g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3(linearSolver);
//L-M优化算法
g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg(block_solver);
//
optimizer.setAlgorithm(algorithm);
optimizer.setVerbose(true);
//添加顶点就是添加优化的参数，这里位姿和特征点都要优化；
//边实际上就是两个参数之间的关系，在这里是两者参数映射的关系
//添加位姿顶点
for(int i = 0;i<2;i++)
{
g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap;
v->setId(i);
v->setFixed(i==0);
v->setEstimate(g2o::SE3Quat());
optimizer.addVertex(v);
}
//添加特征点顶点
for(int i=0;i<points_1.size();i++)
{
g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
Point2d cam = pixel2cam(points_1[i],K);
v->setId(i+2); //已经添加过两个位姿的顶点了
v->setEstimate(Eigen::Vector3d(cam.x,cam.y,1));
v->setMarginalized(true);//把矩阵块分成两个部分，分别求解微量
optimizer.addVertex(v);
}
//添加相机参数
g2o::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);
//添加边,第一帧和第二帧
for(size_t i = 0;i<points_1.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(0)));
edge->setMeasurement(Eigen::Vector2d(points_1[i].x,points_1[i].y));
edge->setRobustKernel(new g2o::RobustKernelHuber());
edge->setInformation(Eigen::Matrix2d::Identity());
edge->setParameterId(0,0);//这句必要
optimizer.addEdge(edge);
}
for(size_t i = 0;i<points_2.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(points_2[i].x,points_2[i].y));
edge->setRobustKernel(new g2o::RobustKernelHuber());
edge->setInformation(Eigen::Matrix2d::Identity());
edge->setParameterId(0,0);
optimizer.addEdge(edge);
}
optimizer.setVerbose(false);
optimizer.initializeOptimization();
optimizer.optimize(20);
//变换矩阵
g2o::VertexSE3Expmap * v1 = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(0)) ;
g2o::VertexSE3Expmap * v2 = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(1)) ;
Eigen::Isometry3d pose1 = v1->estimate();
Eigen::Isometry3d pose2 = v2->estimate();
cout<<"The Pose1 from fram 1=\n"<<pose1.matrix()<<endl;
cout<<"The Pose2 from fram 2(or the frame1 to frame2)=\n"<<pose2.matrix()<<endl;
//****************************BA优化过程*********************
}
CMakeLists.txt:
cmake_minimum_required(VERSION 3.5)
project(testOptimizerG2o LANGUAGES CXX)
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
set(CMAKE_INCLUDE_CURRENT_DIR ON)
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
# OpenCV
find_package( OpenCV 4.3.0 REQUIRED)
# Eigen3
find_package( Eigen3 REQUIRED )
# 寻找G2O
find_package( G2O REQUIRED )
#Sophus
find_package( Sophus REQUIRED)
#Cholmod
find_package( Cholmod REQUIRED)
INCLUDE_DIRECTORIES(
${CHOLMOD_INCLUDE_DIR}
${OpenCV_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${Sophus_INCLUDE_DIRS})
add_executable(testOptimizerG2o main.cpp)
TARGET_LINK_LIBRARIES(testOptimizerG2o
${OpenCV_LIBS}
${Sophus_LIBRARIES}
${CHOLMOD_LIBRARIES}
g2o_core g2o_types_slam3d g2o_solver_csparse g2o_stuff g2o_csparse_extension g2o_types_sim3 g2o_types_sba
)
message(STATUS "OPENCV is :${OpenCV_INCLUDE_DIRS}")
message(STATUS "OPENCV version is :${OpenCV_VERSION}")
message(STATUS "the cholmod is ${CHOLMOD_INCLUDE_DIR} ${CHOLMOD_LIBRARIES}")
完整的文件在参考链接里。
因篇幅问题不能全部显示，请点此查看更多更全内容