0.3版本的VO就是在0.2的基礎上增加了g2o優化: 
1、在visual_odometry.cpp中的poseEstimationPNP()函式中，用PNP求出T_c_r_estimated_後，增加了g2o優化，對位姿進行優化。 
2、同時增加的還有g2o相關的標頭檔案和原始檔：g2o_types.h和g2o_types.cpp

visual_odometry.cpp

    //使用BA優化估計的位姿T
	//1. 位姿6維，觀測點2維
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block;
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
    Block* solver_ptr = new Block( linearSolver );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );
    //2. 頂點是相機位姿pose
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setId ( 0 );
    pose->setEstimate ( g2o::SE3Quat (
        T_c_r_estimated_.rotation_matrix(), 
        T_c_r_estimated_.translation()
    ) );
    optimizer.addVertex ( pose );

    // 3. 邊是重投影誤差
    for ( int i=0; i<inliers.rows; i++ )
    {
        int index = inliers.at<int>(i,0);
        // 3D -> 2D 投影
        EdgeProjectXYZ2UVPoseOnly* edge = new EdgeProjectXYZ2UVPoseOnly();
        edge->setId(i);
        edge->setVertex(0, pose);
		//相機引數
        edge->camera_ = curr_->camera_.get();
		//3D點
        edge->point_ = Vector3d( pts3d[index].x, pts3d[index].y, pts3d[index].z );
		//測量值是2維
        edge->setMeasurement( Vector2d(pts2d[index].x, pts2d[index].y) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        optimizer.addEdge( edge );
    }
    // 4. 執行優化
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    
    T_c_r_estimated_ = SE3 (
        pose->estimate().rotation(),
        pose->estimate().translation()
    );
}
 

目標是優化位姿T，最小化重投影誤差。

高博的g2o_types.h標頭檔案中定義了三種類型的邊，分別是： 
EdgeProjectXYZRGBD 
EdgeProjectXYZRGBDPoseOnly 
EdgeProjectXYZ2UVPoseOnly(重點介紹)

.h

#ifndef MYSLAM_G2O_TYPES_H
#define MYSLAM_G2O_TYPES_H

#include "myslam/common_include.h"
#include "camera.h"
#include <g2o/g2o.h>

namespace myslam
{

class EdgeProjectXYZRGBD : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    virtual void computeError();
    virtual void linearizeOplus();
    virtual bool read( std::istream& in ){}
    virtual bool write( std::ostream& out) const {}
    
};
class EdgeProjectXYZRGBDPoseOnly: public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap >
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    // Error: measure = R*point+t
    virtual void computeError();
    virtual void linearizeOplus();
    
    virtual bool read( std::istream& in ){}
    virtual bool write( std::ostream& out) const {}
    
    Vector3d point_;
};

//只優化位姿pose,沒有優化點  3D—2D
class EdgeProjectXYZ2UVPoseOnly: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap >
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    //計算誤差和線性增量函式（雅克比矩陣J）
    virtual void computeError();
    virtual void linearizeOplus();
    
    virtual bool read( std::istream& in ){}
    virtual bool write(std::ostream& os) const {};
    // 3D點和相機模型為成員變數
    Vector3d point_;
    Camera* camera_;
};

}

#endif // MYSLAM_G2O_TYPES_H
 

.cpp

#include "myslam/g2o_types.h"

namespace myslam
//1. 計算重投影誤差
void EdgeProjectXYZ2UVPoseOnly::computeError()
{
	//1. 相機位姿為頂點
    const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
	//2. 誤差是測量值減去估計值，估計值T*p,
    _error = _measurement - camera_->camera2pixel ( 
        pose->estimate().map(point_) );
}
//2. 計算線性增量函式，雅克比矩陣J
void EdgeProjectXYZ2UVPoseOnly::linearizeOplus()
{
	//頂點取出位姿
    g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] );
	//位姿構造四元數形式T
    g2o::SE3Quat T ( pose->estimate() );
	//變換後的3D座標 T*p
    Vector3d xyz_trans = T.map ( point_ );
	//變幻後的3D點xyz座標
    double x = xyz_trans[0];
    double y = xyz_trans[1];
    double z = xyz_trans[2];
    double z_2 = z*z;
	// 雅克比矩陣2*6
    _jacobianOplusXi ( 0,0 ) =  x*y/z_2 *camera_->fx_;
    _jacobianOplusXi ( 0,1 ) = - ( 1+ ( x*x/z_2 ) ) *camera_->fx_;
    _jacobianOplusXi ( 0,2 ) = y/z * camera_->fx_;
    _jacobianOplusXi ( 0,3 ) = -1./z * camera_->fx_;
    _jacobianOplusXi ( 0,4 ) = 0;
    _jacobianOplusXi ( 0,5 ) = x/z_2 * camera_->fx_;

    _jacobianOplusXi ( 1,0 ) = ( 1+y*y/z_2 ) *camera_->fy_;
    _jacobianOplusXi ( 1,1 ) = -x*y/z_2 *camera_->fy_;
    _jacobianOplusXi ( 1,2 ) = -x/z *camera_->fy_;
    _jacobianOplusXi ( 1,3 ) = 0;
    _jacobianOplusXi ( 1,4 ) = -1./z *camera_->fy_;
    _jacobianOplusXi ( 1,5 ) = y/z_2 *camera_->fy_;
}

}