G2O图优化基础和SLAM的Bundle Adjustment(光束法平差)

class <派生类名>:<继承方式><基类名>
{ 　　
	<派生类新定义成员> 　　
};

class Foo
{
  //...
  Eigen::Vector2d v;
  //...
};
//...
Foo *foo = new Foo();

 class Foo
{
  ...
  Eigen::Vector2d v;
  ...
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

/**
 * \brief Templatized BaseVertex
 *
 * Templatized BaseVertex
 * D  : minimal dimension of the vertex, e.g., 3 for rotation in 3D
 * T  : internal type to represent the estimate, e.g., Quaternion for rotation in 3D
 */
  template <int D, typename T>
  class BaseVertex : public OptimizableGraph::Vertex {
  	//...
  }

class CurveFittingVertex: public g2o::BaseVertex<4, Eigen::Vector4d>{
	//...
}

        /**
         * update the position of the node from the parameters in v.
         * Implement in your class!
         */
        virtual void oplusImpl(const number_t* v) = 0;

        //! sets the node to the origin (used in the multilevel stuff)
        virtual void setToOriginImpl() = 0;
        //! read the vertex from a stream, i.e., the internal state of the vertex
        virtual bool read(std::istream& is) = 0;
        //! write the vertex to a stream
        virtual bool write(std::ostream& os) const = 0;

static_cast <type-id> (expression)

for(auto e:edges){
    //code...
  }

cmake_minimum_required(VERSION 2.6)
project(g2o_test)

set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

# 寻找G2O
include_directories( 
    ${G2O_INCLUDE_DIRS}
    "/usr/include/eigen3"
    "/usr/include/suitesparse"
)

# OpenCV
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS} )

add_executable(g2o_test main.cpp)

# 与G2O和OpenCV链接
target_link_libraries( g2o_test 
    ${OpenCV_LIBS}
    g2o_core g2o_stuff
)

install(TARGETS g2o_test RUNTIME DESTINATION bin)

#include <iostream>
#include <g2o/core/base_vertex.h>//顶点类型
#include <g2o/core/base_unary_edge.h>//一元边类型
#include <g2o/core/block_solver.h>//求解器的实现,主要来自choldmod, csparse
#include <g2o/core/optimization_algorithm_levenberg.h>//列文伯格－马夸尔特
#include <g2o/core/optimization_algorithm_gauss_newton.h>//高斯牛顿法
#include <g2o/core/optimization_algorithm_dogleg.h>//Dogleg（狗腿方法）
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>//矩阵库
#include <opencv2/core/core.hpp>//opencv2
#include <cmath>//数学库
#include <chrono>//时间库

using namespace std;
      
//定义曲线模型顶点，这也是我们的待优化变量
//这个顶点类继承于基础顶点类，基础顶点类是个模板类，模板参数表示优化变量的维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<4, Eigen::Vector4d>
{
public:
  //这在前面说过了，因为类中含有Eigen对象，为了防止内存不对齐，加上这句宏定义
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  
  //在基类中这是个=0的虚函数，所以必须重新定义
  //它用于重置顶点，例如全部重置为0
  virtual void setToOriginImpl()
  {
    //这里的_estimate是基类中的变量，直接用了
    _estimate << 0,0,0,0;
  }
  
  //这也是一个必须要重定义的虚函数
  //它的用途是对顶点进行更新，对应优化中X(k+1)=Xk+Dx
  //需要注意的是虽然这里的更新是个普通的加法，但并不是所有情况都是这样
  //例如相机的位姿，其本身没有加法运算
  //这时就需要我们自己定义"增量如何加到现有估计上"这样的算法了
  //这也就是为什么g2o没有为我们写好的原因
  virtual void oplusImpl( const double* update )
  {
    //注意这里的Vector4d，d是double的意思，f是float的意思
    _estimate += Eigen::Vector4d(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
  double _x;
  
  //这种写法应该有一种似曾相识的感觉
  //在Ceres里定义代价函数结构体的时候，那里的构造函数也用了这种写法
  CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}

  //计算曲线模型误差 
  void computeError()
  {
    //顶点，用到了编译时类型检查
    //_vertices是基类中的成员变量
    const CurveFittingVertex* v = static_cast<const CurveFittingVertex*>(_vertices[0]);
    //获取顶点的优化变量
    const Eigen::Vector4d abcd = v->estimate();
    //_error、_measurement都是基类中的成员变量
    _error(0,0) = _measurement - std::exp(abcd(0,0)*_x*_x*_x + abcd(1,0)*_x*_x + abcd(2,0)*_x + abcd(3,0));
  }
  
  //存盘和读盘：留空
  virtual bool read( istream& in ) {}
  virtual bool write( ostream& out ) const {}
};
      
int main( int argc, char** argv )
{
  //待估计函数为y=exp(3.5x^3+1.6x^2+0.3x+7.8)
  double a=3.5, b=1.6, c=0.3, d=7.8;
  int N=100;
  double w_sigma=1.0;
  cv::RNG rng;
  
  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*x + b*x*x + c*x + d) + rng.gaussian (w_sigma));
    cout<<x_data[i]<<"\t"<<y_data[i]<<endl;
  }
  
  //构建图优化，先设定g2o
  //矩阵块：每个误差项优化变量维度为4，误差值维度为1
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<4,1>> Block;

  // Step1 选择一个线性方程求解器
  std::unique_ptr<Block::LinearSolverType> linearSolver (new g2o::LinearSolverDense<Block::PoseMatrixType>());
  // Step2 选择一个稀疏矩阵块求解器
  std::unique_ptr<Block> solver_ptr (new Block(std::move(linearSolver)));
  // Step3 选择一个梯度下降方法，从GN、LM、DogLeg中选
  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr));
  //g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( std::move(solver_ptr));
  //g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg(std::move(solver_ptr));

  //稀疏优化模型
  g2o::SparseOptimizer optimizer;
  //设置求解器
  optimizer.setAlgorithm(solver);
  //打开调试输出
  optimizer.setVerbose(true);
          
  //往图中增加顶点
  CurveFittingVertex* v = new CurveFittingVertex();
  //这里的(0,0,0,0)可以理解为顶点的初值了，不同的初值会导致迭代次数不同，可以试试
  v->setEstimate(Eigen::Vector4d(0,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);
    //设置连接的顶点，注意使用方式
    //这里第一个参数表示边连接的第几个节点(从0开始)，第二个参数是该节点的指针
    edge->setVertex(0, v);
    //观测数值
    edge->setMeasurement(y_data[i]);
    //信息矩阵：协方差矩阵之逆，这里各边权重相同。这里Eigen的Matrix其实也是模板类
    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::Vector4d abc_estimate = v->estimate();
  cout<<"estimated model: "<<abc_estimate.transpose()<<endl;
  
  return 0;
}

//
// Created by root on 18-4-9.
//

#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"

#include "g2o/types/slam2d/vertex_se2.h"
#include "g2o/types/slam3d/vertex_se3.h"

#include <iostream>

//G2O_USE_TYPE_GROUP保证能保证graph的load和save的成功
//也就是optimizer.load和optimizer.save在执行时有相对的类型对应.
//slam3d和slam2d对应不同的顶点和边的类型.这里都registration.
G2O_USE_TYPE_GROUP(slam3d);
G2O_USE_TYPE_GROUP(slam2d);

using namespace std;
using namespace g2o;

#define MAXITERATION 10


int main(){

    char* path = "sphere2500.g2o";

    unique_ptr<BlockSolverX::LinearSolverType> linearSolver(new LinearSolverCholmod<BlockSolverX ::PoseMatrixType>());
    unique_ptr<BlockSolverX> blockSolverX(new BlockSolverX(move(linearSolver)));
    OptimizationAlgorithmLevenberg* solver = new OptimizationAlgorithmLevenberg(move(blockSolverX));

    SparseOptimizer optimizer;

    if(!optimizer.load(path)){
        cout<<"Error loading file."<<endl;
        return -1;
    }else{
        cout<<"Loaded "<<optimizer.vertices().size()<<" vertices"<<endl;
        cout<<"Loaded "<<optimizer.edges().size()<<" edges."<<endl;
    }

    //这里这样做的目的是固定第一个点的位姿，当然不固定也可以，但最终优化的效果会有差别
    //for sphere2500
    VertexSE3* firstRobotPose = dynamic_cast<VertexSE3*>(optimizer.vertex(0));
    //for manhattanOlson3500
    //VertexSE2* firstRobotPose = dynamic_cast<VertexSE2*>(optimizer.vertex(0));
    firstRobotPose->setFixed(true);

    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    cout<<"开始优化..."<<endl;
    optimizer.initializeOptimization();
    optimizer.optimize(MAXITERATION);
    cout<<"优化完成..."<<endl;
    optimizer.save("after.g2o");
    optimizer.clear();

    return 0;
}

cmake_minimum_required(VERSION 3.10)
project(bundle)

set(CMAKE_CXX_STANDARD 11)

# 添加cmake模块以使用ceres库
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

# 寻找G2O
include_directories(
        ${G2O_INCLUDE_DIRS}
        "/usr/include/eigen3"
        "/usr/include/suitesparse"
)

# OpenCV
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRS})

add_executable(bundle main.cpp ORBMatching.cpp ORBMatching.h sphere2500.cpp)

# 与G2O和OpenCV链接
target_link_libraries(bundle
        g2o_core g2o_stuff g2o_cli
        GL GLU cholmod
        g2o_incremental g2o_interactive g2o_interface g2o_parser
        g2o_solver_cholmod g2o_solver_csparse g2o_solver_dense g2o_solver_pcg
        g2o_types_icp g2o_types_sba g2o_types_sim3 g2o_types_slam2d g2o_types_slam3d
        ${OpenCV_LIBS}
        ${QT_LIBRARIES}
        ${QT_QTOPENGL_LIBRARY}
        ${GLUT_LIBRARY}
        ${OPENGL_LIBRARY}
        )

#include<iostream>
#include"ORBMatching.h"
#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>
#include <chrono>

using namespace std;

void epipolar(vector<Point2f> pts1, vector<Point2f> pts2, double fx, double fy, double dx, double dy) {

    cout << "--------epipolar constraint--------" << endl;
    Point2d principal_point(dx, dy);
    Mat essential_matrix;
    double f = (fx + fy) / 2.0;
    essential_matrix = findEssentialMat(pts1, pts2, f, principal_point, RANSAC);
    Mat R, t;
    recoverPose(essential_matrix, pts1, pts2, R, t, f, principal_point);
    cout << "R:" << endl << R << endl;
    cout << "t:" << endl << t << endl;
    cout << "--------epipolar constraint--------" << endl;
    return;
}

void graph_optimization(vector<Point2f> &pts1, vector<Point2f> &pts2, double fx, double fy, double cx, double cy) {
    cout << "--------bundle adjustment--------" << endl;

    //矩阵块：每个误差项优化变量维度为6，误差值维度为3
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> Block;
    // Step1 选择一个线性方程求解器
    std::unique_ptr<Block::LinearSolverType> linearSolver(new g2o::LinearSolverCholmod<Block::PoseMatrixType>());
    // Step2 选择一个稀疏矩阵块求解器
    std::unique_ptr<Block> solver_ptr(new Block(std::move(linearSolver)));
    // Step3 选择一个梯度下降方法，从GN、LM、DogLeg中选
    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr));

    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    //添加两个位姿节点，默认值为单位Pose，并且将第一个位姿节点也就是第一帧的位姿固定
    for (int i = 0; i < 2; i++) {
        g2o::VertexSE3Expmap *v = new g2o::VertexSE3Expmap();
        v->setId(i);
        if (i == 0) {
            v->setFixed(true);
        }
        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(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);

    //添加第一帧中的边
    vector<g2o::EdgeProjectXYZ2UV *> edges;
    for (size_t i = 0; i < pts1.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(pts1[i].x, pts1[i].y));
        edge->setInformation(Eigen::Matrix2d::Identity());
        edge->setParameterId(0, 0);
        edge->setRobustKernel(new g2o::RobustKernelHuber());

        optimizer.addEdge(edge);
        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;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//计时
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();//结束计时
    cout << "优化完毕" << endl;

    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

    //输出变换矩阵T
    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;
    }

    //估计inliner的个数
    int inliners = 0;
    for (auto e:edges) {
        e->computeError();
        //chi2()如果很大，说明此边的值与其它边很不相符
        if (e->chi2() > 1) {
            cout << "error = " << e->chi2() << endl;
        } else {
            inliners++;
        }
    }
    cout << "inliners in total points:" << inliners << "/" << pts1.size() + pts2.size() << endl;
    optimizer.save("ba.g2o");
    cout << "--------bundle adjustment--------" << endl;
    return;
};

double cx = 1535.89;
double cy = 2046.97;
double fx = 1536.2;
double fy = 1536.2;
string img_path1 = "/root/satellite_slam_data/tianjin/jpg2/VBZ1_201801051905_002_0002_L1A.jpg";
string img_path2 = "/root/satellite_slam_data/tianjin/jpg2/VBZ1_201801051905_002_0003_L1A.jpg";


int main(int argc, char **argv) {

    vector<Point2f> pts1, pts2;
    getMatchPoints(img_path1, img_path2, pts1, pts2);
    graph_optimization(pts1, pts2, fx, fy, cx, cy);
    epipolar(pts1, pts2, fx, fy, cx, cy);

    return 0;
}