[学习SLAM]G2O的在SLAM中使用示例[实践篇] 曲线拟合示例1 ,优化 PnP 的结果示例2,

最新推荐文章于 2022-08-19 18:16:54 发布

苏源流

最新推荐文章于 2022-08-19 18:16:54 发布

阅读量538

点赞数

分类专栏： SLAM 位姿测量

本文链接：https://blog.csdn.net/KYJL888/article/details/87727119

版权

位姿测量同时被 2 个专栏收录

73 篇文章 37 订阅

订阅专栏

SLAM

57 篇文章 21 订阅

订阅专栏

图优化理论简介
我们已经介绍了非线性最小二乘的求解方式。它们是由很多个误差项之和组成的。然而,仅有一组优化变量和许多个误差项,我们并不清楚它们之间的关联。比方说,某一个优化变量 x j 存在于多少个误差项里呢?我们能保证对它的优化是有意义的吗?,我们希望能够直观地看到该优化问题长什么样。于是,就说到了图优化。图优化,是把优化问题表现成图(Graph)的一种方式。这里的图是图论意义上的图。一个图由若干个顶点(Vertex),以及连接着这些节点的边(Edge)组成。进而,用顶点表示优化变量,用边表示误差项。

曲线拟合示例1

代码

#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; 

// 曲线模型的顶点，模板参数：优化变量维度和数据类型--Vertex顶点  
//(//第一个参数3表示update指针指向的数组大小，Eigen::Vector3d为_estimate的类型)
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>  //基顶点 BaseVertex
{
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 {}
};

// 误差模型 模板参数：观测值维度，类型，连接顶点类型--Edge边
//(//1为_error的大小，double为观测值_measurement的大小，CurveFittingVetex为一元边上的一个顶点)
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;
    }
    //步骤就是，先把复杂的矩阵块类型在这里定义为Block.然后定义线性方程求解器linearsolver,再定义矩阵块求解器solver_ptr和linearsolver有关，再定义梯度下降方法solver和solver_ptr有关。
    // 构建图优化，先设定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 );//6.2.3 Levenberg-Marquadt
//     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 );                // 设置连接的顶点,0是  v->setId(0) ??
        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;
}

// 梯度下降方法，从GN, LM, DogLeg 中选
 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );//6.2.3 Levenberg-Marquadt
 // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
 // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );

solve time cost = 0.000989289 seconds.

estimated model: 0.890912 2.1719 0.943629

solve time cost = 0.00265157 seconds.

estimated model: 394.439 -169.761 3.84695

solve time cost = 0.00105172 seconds.

estimated model: 0.890911 2.1719 0.943629

BA优化 PnP 的结果示例2

我们来尝试RANSAC PnP 加上迭代优化的方式估计相机位姿,看看是否对前一节的效果有所改进。非线性优化问题的求解。由于本节的目标是估计位姿而非结构,我们以相机位姿 ξ 为优化变量,通过最小化重投影误差,来构建优化问题。与之前一样,我们自定义一个 g2o 中的优化边。它只优化一个位姿,因此是一个一元边。

由之前的PnP,可以求出一个R,t,K。而且空间点的世界坐标知道，第二个相机位姿的像素坐标也是知道的。就可以利用它们进行优化。
首先确定变量为const vector<Point3f> points_3d,const vector<Point2f>,const Mat& K,Mat& R,Mat& t.
因为之后放进去的pts_3d,pts_2d是我们自己计算出来的，所以不用用&。
开始函数：

1.初始化g2o.这个可以是固定的方式。基本上都是相似的。

定义矩阵块的类型为Block.这样方便之后的写入。它有两个参数，第一个参数为优化变量的维度，这里为6，第二个参数为误差值的维度，这里为3。
步骤就是，先把复杂的矩阵块类型在这里定义为Block.然后定义线性方程求解器linearsolver,再定义矩阵块求解器solver_ptr和linearsolver有关，再定义梯度下降方法solver和solver_ptr有关。

    // using bundle adjustment to optimize the pose 
    
    //步骤就是，先把复杂的矩阵块类型在这里定义为Block.然后定义线性方程求解器linearsolver,再定义矩阵块求解器solver_ptr和linearsolver有关，再定义梯度下降方法solver和solver_ptr有关。
    
    //定义矩阵块的类型为Block.这样方便之后的写入。它有两个参数，第一个参数为优化变量的维度，这里为6，第二个参数为误差值的维度，这里为2。
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block;
    //定义线性求解器的类型linearsolver要带指针，有稠密Dense和CSparse两种类型。前面要加new.参数为矩阵块里的位姿矩阵类型
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
    //Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();//XXX
    //定义矩阵块的求解器solver_ptr。要带指针。也要带new.g2o赋初值的都要带new.
    Block* solver_ptr = new Block( linearSolver );
    //定义梯度下降方法为levenberg方法solver.
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    //定义图模型optimizer.类型为g2o::SparseOptimizer.
    g2o::SparseOptimizer optimizer;
    //然后设置图模型的求解器，也就是之前定义的梯度下降方法。
    optimizer.setAlgorithm ( solver );
    // //    设置要不要打开调试输出，设置为true.这个选项有的时候可以不设
    //    optimizer.setVerbose(true);

2.设置顶点有关的东西，这里有两个顶点，一个是李代数位姿pose，一个是空间点位置p。


//首先确定变量为const vector<Point3f> points_3d,const vector<Point2f>,const Mat& K,Mat& R,Mat& t.
//因为之后放进去的pts_3d,pts_2d是我们自己计算出来的，所以不用 用&
void VisualOdometry::poseEstimationPnP()
{
    // construct the 3d 2d observations
    vector<cv::Point3f> pts3d;
    vector<cv::Point2f> pts2d;
    
    for ( cv::DMatch m:feature_matches_ )
    {
        pts3d.push_back( pts_3d_ref_[m.queryIdx] );
        pts2d.push_back( keypoints_curr_[m.trainIdx].pt );
    }
    
    Mat K = ( cv::Mat_<double>(3,3)<<
        ref_->camera_->fx_, 0, ref_->camera_->cx_,
        0, ref_->camera_->fy_, ref_->camera_->cy_,
        0,0,1
    );
    Mat rvec, tvec, inliers;
    cv::solvePnPRansac( pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers );
    num_inliers_ = inliers.rows;
    cout<<"pnp inliers: "<<num_inliers_<<endl;
    //优化 PnP 的结果 将优pnpRansac的初值给T_c_r_estimated
    //一个是Estimate,这里是由R,t组成的SE3Quat形式。只要把R,t放进去就可以了。
    //但是R必须是矩阵形式。由R.at<double>(0,0)等转化。t必须是向量，由t.at<double>(0,0)等转化。
    T_c_r_estimated_ = SE3(
        SO3(rvec.at<double>(0,0), rvec.at<double>(1,0), rvec.at<double>(2,0)), 
        Vector3d( tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0))
    );
    


    // using bundle adjustment to optimize the pose 


    /*初始化g2o.这个可以是固定的方式*/
    //步骤就是，先把复杂的矩阵块类型在这里定义为Block.然后定义线性方程求解器linearsolver,再定义矩阵块求解器solver_ptr和linearsolver有关，再定义梯度下降方法solver和solver_ptr有关。

    //定义矩阵块的类型为Block.这样方便之后的写入。它有两个参数，第一个参数为优化变量的维度，这里为6，第二个参数为误差值的维度，这里为2。
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block;
    //定义线性求解器的类型linearsolver要带指针，有稠密Dense和CSparse两种类型。前面要加new.参数为矩阵块里的位姿矩阵类型
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
    //Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();//XXX
    //定义矩阵块的求解器solver_ptr。要带指针。也要带new.g2o赋初值的都要带new.
    Block* solver_ptr = new Block( linearSolver );
    //定义梯度下降方法为levenberg方法solver.
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    //定义图模型optimizer.类型为g2o::SparseOptimizer.
    g2o::SparseOptimizer optimizer;
    //然后设置图模型的求解器，也就是之前定义的梯度下降方法。
    optimizer.setAlgorithm ( solver );
    // //    设置要不要打开调试输出，设置为true.这个选项有的时候可以不设
    //    optimizer.setVerbose(true);


    /*设置顶点有关的东西，这里有两个顶点，一个是李代数位姿pose，一个是空间点位置p。*/

    //李代数位姿顶点类型为g2o::VertexSE3Expmap*,初值为new g2o::VertexSE3Expmap();
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    //顶点要设置两个东西，一个是Id,一般设为0，
    pose->setId ( 0 );
    //一个是Estimate,这里是由R,t组成的SE3Quat形式。只要把R,t放进去就可以了。
    //但是R必须是矩阵形式。由之前的R.at<double>(0,0)等转化。t必须是向量，由t.at<double>(0,0)等转化。
    pose->setEstimate ( g2o::SE3Quat (
        T_c_r_estimated_.rotation_matrix(), 
        T_c_r_estimated_.translation()
    ) );
    optimizer.addVertex ( pose );

    // edges
    //边肯定在一个for循环里,边的类型为g2o::EdgeProjectXYZ2UV,初值为new g2o::EdgeProjectXYZ2UV().
    for ( int i=0; i<inliers.rows; i++ )
    {
        int index = inliers.at<int>(i,0);
        // 3D -> 2D projection
        EdgeProjectXYZ2UVPoseOnly* edge = new EdgeProjectXYZ2UVPoseOnly();
        //设置id为index,
        edge->setId(i);
        //设置顶点0为位姿。
        edge->setVertex(0, pose);
        //相机参数camera.类型为g2o::CameraParmetersI,值为K.at<double>(0,0),和cx,cy组成的2维向量,
        edge->camera_ = curr_->camera_.get();
        edge->point_ = Vector3d( pts3d[index].x, pts3d[index].y, pts3d[index].z );
        edge->setMeasurement( Vector2d(pts2d[index].x, pts2d[index].y) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        optimizer.addEdge( edge );
    }
    //求解
    //求解就特别简单，先把图模型给初始化一下。
    optimizer.initializeOptimization();
    //直接用优化函数optimize()进行优化
    optimizer.optimize(10);
    //由优化可以得到位姿的估计值
    //pose->estimate.要想把它转成常见形式，可以用欧式变换矩阵Eigen::Isometry3d，是4*4矩阵，T=Eigen::Isometry3d(pose->estimate()).matrix
    T_c_r_estimated_ = SE3 (
        pose->estimate().rotation(),
        pose->estimate().translation()
    );
}


#ifndef MYSLAM_G2O_TYPES_H
#define MYSLAM_G2O_TYPES_H

#include "myslam/common_include.h"
#include "camera.h"

#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/types/sba/types_six_dof_expmap.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.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 {}
    
};
//李代数位姿顶点类型为g2o::VertexSE3Expmap*,初值为new g2o::VertexSE3Expmap();
// only to optimize the pose, no point
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_;
};
//李代数位姿顶点类型为g2o::VertexSE3Expmap
class EdgeProjectXYZ2UVPoseOnly: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, 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& os) const {};
    
    Vector3d point_;
    Camera* camera_;
};

}


#endif // MYSLAM_G2O_TYPES_H

#include "myslam/g2o_types.h"
//以相机位姿 ξ 为优化变量,通过最小化重投影误差,来构建优化问题
namespace myslam
{
void EdgeProjectXYZRGBD::computeError()
{
    const g2o::VertexSBAPointXYZ* point = static_cast<const g2o::VertexSBAPointXYZ*> ( _vertices[0] );
    const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[1] );
    _error = _measurement - pose->estimate().map ( point->estimate() );
}

void EdgeProjectXYZRGBD::linearizeOplus()
{
    g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap *> ( _vertices[1] );
    g2o::SE3Quat T ( pose->estimate() );
    g2o::VertexSBAPointXYZ* point = static_cast<g2o::VertexSBAPointXYZ*> ( _vertices[0] );
    Eigen::Vector3d xyz = point->estimate();
    Eigen::Vector3d xyz_trans = T.map ( xyz );
    double x = xyz_trans[0];
    double y = xyz_trans[1];
    double z = xyz_trans[2];

    _jacobianOplusXi = - T.rotation().toRotationMatrix();

    _jacobianOplusXj ( 0,0 ) = 0;
    _jacobianOplusXj ( 0,1 ) = -z;
    _jacobianOplusXj ( 0,2 ) = y;
    _jacobianOplusXj ( 0,3 ) = -1;
    _jacobianOplusXj ( 0,4 ) = 0;
    _jacobianOplusXj ( 0,5 ) = 0;

    _jacobianOplusXj ( 1,0 ) = z;
    _jacobianOplusXj ( 1,1 ) = 0;
    _jacobianOplusXj ( 1,2 ) = -x;
    _jacobianOplusXj ( 1,3 ) = 0;
    _jacobianOplusXj ( 1,4 ) = -1;
    _jacobianOplusXj ( 1,5 ) = 0;

    _jacobianOplusXj ( 2,0 ) = -y;
    _jacobianOplusXj ( 2,1 ) = x;
    _jacobianOplusXj ( 2,2 ) = 0;
    _jacobianOplusXj ( 2,3 ) = 0;
    _jacobianOplusXj ( 2,4 ) = 0;
    _jacobianOplusXj ( 2,5 ) = -1;
}

void EdgeProjectXYZRGBDPoseOnly::computeError()
{
    const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
    _error = _measurement - pose->estimate().map ( point_ );
}

void EdgeProjectXYZRGBDPoseOnly::linearizeOplus()
{
    g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] );
    g2o::SE3Quat T ( pose->estimate() );
    Vector3d xyz_trans = T.map ( point_ );
    double x = xyz_trans[0];
    double y = xyz_trans[1];
    double z = xyz_trans[2];

    _jacobianOplusXi ( 0,0 ) = 0;
    _jacobianOplusXi ( 0,1 ) = -z;
    _jacobianOplusXi ( 0,2 ) = y;
    _jacobianOplusXi ( 0,3 ) = -1;
    _jacobianOplusXi ( 0,4 ) = 0;
    _jacobianOplusXi ( 0,5 ) = 0;

    _jacobianOplusXi ( 1,0 ) = z;
    _jacobianOplusXi ( 1,1 ) = 0;
    _jacobianOplusXi ( 1,2 ) = -x;
    _jacobianOplusXi ( 1,3 ) = 0;
    _jacobianOplusXi ( 1,4 ) = -1;
    _jacobianOplusXi ( 1,5 ) = 0;

    _jacobianOplusXi ( 2,0 ) = -y;
    _jacobianOplusXi ( 2,1 ) = x;
    _jacobianOplusXi ( 2,2 ) = 0;
    _jacobianOplusXi ( 2,3 ) = 0;
    _jacobianOplusXi ( 2,4 ) = 0;
    _jacobianOplusXi ( 2,5 ) = -1;
}

void EdgeProjectXYZ2UVPoseOnly::computeError()
{
    const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
    _error = _measurement - camera_->camera2pixel ( 
        pose->estimate().map(point_) );
}

void EdgeProjectXYZ2UVPoseOnly::linearizeOplus()
{
    g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] );
    g2o::SE3Quat T ( pose->estimate() );
    Vector3d xyz_trans = T.map ( point_ );
    double x = xyz_trans[0];
    double y = xyz_trans[1];
    double z = xyz_trans[2];
    double z_2 = z*z;

    _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_;
}


}

李代数位姿顶点类型为g2o::VertexSE3Expmap*,初值为new g2o::VertexSE3Expmap();
顶点要设置两个东西，一个是Id,一般设为0，一个是Estimate,这里是由R,t组成的SE3Quat形式。只要把R,t放进去就可以了。但是R必须是矩阵形式。由之前的R.at<double>(0,0)等转化。t必须是向量，由t.at<double>(0,0)等转化。
至于空间点式的顶点，就需要一个for循环了，因为空间点有很多啊。
for(const Point3f p:points_3d)
顶点设为point,类型为g2o::VertexSBAPointXYZ,初值为new g2o::VertexSBAPointXYZ();
Id设为index++,估计值设为p.x,p.y,p.z的组成的向量形式。还设置了一个setMarginalized(true).
3相机参数
里面还用到了相机参数camera.类型为g2o::CameraParmetersI,值为K.at<double>(0,0),和cx,cy组成的2维向量，0组成的。
Id设置为0,
4.边。
重点是边了。边肯定在一个for循环里，这里是for(const Point2f p:points_2d)
边的类型为g2o::EdgeProjectXYZ2UV,初值为new g2o::EdgeProjectXYZ2UV().
设置id为index,
设置顶点0为空间点位置，而且里面用了一个dyanmic_cast把图模型的顶点坐标设置为空间点顶点类型。
edge->setVertex(0,dynamic_cat<g2o::VertexSBAPointXYZ*>(optimizer.vertex(index)));
设置顶点1为位姿。
edge->setVertex(1,pose);
设置测量值为p.x,p.y组成的2维向量模式。
设置参数ID为(0,0).
edge->setMeasurement(Eigen::Vector2d(p.x,p.y));
edge->setParameterId(0,0);
设置信息矩阵为2维的单位矩阵。信息矩阵的维度是根据误差值的维度定的。
5.求解
求解就特别简单，先把图模型给初始化一下。
optimizer.IntializeOptimization();
直接用优化函数optimize()进行优化。
optimizer.optimize(100)
由优化可以得到位姿的估计值。pose->estimate.要想把它转成常见形式，可以用欧式变换矩阵Eigen::Isometry3d，是4*4矩阵，
T=Eigen::Isometry3d(pose->estimate()).matrix

初始化g2o.这个可以是固定的方式。基本上都是相似的。

//顶点

// 往图中增加边

说明:

//定义矩阵块的类型为Block.这样方便之后的写入。它有两个参数，第一个参数为优化变量的维度，这里为6(R,T)，第二个参数为误差值的维度，这里为2(重投影x,y 没以和表示吗)。//3
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> Block;//6,1 6,2 6,3

其中BlockSolverTraits<6,1>> Block 可以为6,1； 6,2；6,3结果相同. 如果为5 报错,如果为7可以,但结果稍微不同

稀疏直接法

计算光度误差的边,它的雅可比矩阵

g20


bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
{
    // 初始化g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;  // 求解的向量是6＊1的
    DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
    DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );
    optimizer.setVerbose( true );

    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );
    pose->setId ( 0 );
    optimizer.addVertex ( pose );

    // 添加边
    int id=1;
    for ( Measurement m: measurements )
    {
        EdgeSE3ProjectDirect* edge = new EdgeSE3ProjectDirect (
            m.pos_world,
            K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray
        );
        edge->setVertex ( 0, pose );
        edge->setMeasurement ( m.grayscale );
        edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );
        edge->setId ( id++ );
        optimizer.addEdge ( edge );
    }
    cout<<"edges in graph: "<<optimizer.edges().size() <<endl;
    optimizer.initializeOptimization();
    optimizer.optimize ( 30 );
    Tcw = pose->estimate();
}

雅可比矩阵

// project a 3d point into an image plane, the error is photometric error
// an unary edge with one vertex SE3Expmap (the pose of camera)
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeSE3ProjectDirect() {}

    EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image )
        : x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image )
    {}

    virtual void computeError()
    {
        const VertexSE3Expmap* v  =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
        float x = x_local[0]*fx_/x_local[2] + cx_;
        float y = x_local[1]*fy_/x_local[2] + cy_;
        // check x,y is in the image
        if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
        {
            _error ( 0,0 ) = 0.0;
            this->setLevel ( 1 );
        }
        else
        {
            _error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;
        }
    }

    // plus in manifold
    virtual void linearizeOplus( )
    {
        if ( level() == 1 )
        {
            _jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
            return;
        }
        VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ );   // q in book _r*xyz + _t;

        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double invz = 1.0/xyz_trans[2];
        double invz_2 = invz*invz;

        float u = x*fx_*invz + cx_;
        float v = y*fy_*invz + cy_;

        // jacobian from se3 to u,v
        // NOTE that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation
        Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;

        jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
        jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
        jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
        jacobian_uv_ksai ( 0,3 ) = invz *fx_;
        jacobian_uv_ksai ( 0,4 ) = 0;
        jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;

        jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
        jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
        jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
        jacobian_uv_ksai ( 1,3 ) = 0;
        jacobian_uv_ksai ( 1,4 ) = invz *fy_;
        jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;

        Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;

        jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;
        jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;

        _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;
    }

    // dummy read and write functions because we don't care...
    virtual bool read ( std::istream& in ) {}
    virtual bool write ( std::ostream& out ) const {}

protected:
    // get a gray scale value from reference image (bilinear interpolated)
    inline float getPixelValue ( float x, float y )
    {
        uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];
        float xx = x - floor ( x );
        float yy = y - floor ( y );
        return float (
                   ( 1-xx ) * ( 1-yy ) * data[0] +
                   xx* ( 1-yy ) * data[1] +
                   ( 1-xx ) *yy*data[ image_->step ] +
                   xx*yy*data[image_->step+1]
               );
    }
public:
    Eigen::Vector3d x_world_;   // 3D point in world frame
    float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics
    cv::Mat* image_=nullptr;    // reference image
};