利用协方差约束单个方向的g2o写法

由于匹配结果有时候只约束单个方向，比如法向，因此需要考虑只约束部分方向的误差方程写法，虽然可以使用自定义边，但经过测试，自定义欧式距离边加上位姿的求解通常不能收敛，因此这里均采用位姿的误差边形式，借用协方差，来实现仅约束部分方向的实现。  注意比较关键的是协方差方向是在局部坐标系下，而非世界坐标系，原因是位姿相减，其平移向量残差是在被减位姿所在坐标系下的。

g2o::SparseOptimizer optimizer;
    std::unique_ptr<g2o::BlockSolverX::LinearSolverType> linearSolver(new g2o::LinearSolverCholmod<g2o::BlockSolverX::PoseMatrixType>());
    std::unique_ptr<g2o::BlockSolverX> blockSolver(new g2o::BlockSolverX(std::move(linearSolver)));
    g2o::OptimizationAlgorithmLevenberg* optimizationAlgorithm = new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver));

    optimizer.setAlgorithm(optimizationAlgorithm);
    optimizer.setVerbose(true);
    g2o::ParameterSE3Offset* cameraOffset = new g2o::ParameterSE3Offset;
    cameraOffset->setId(0);
    optimizer.addParameter(cameraOffset);

    Eigen::Matrix<double, 6, 6> inf = Eigen::Matrix<double, 6, 6>::Zero();

    inf(0, 0) = 1.0; inf(0, 1) = 0.0; inf(0, 2) = 0.0;
    inf(1, 0) = 0.0; inf(1, 1) = 1.0; inf(1, 2) = 0.0;
    inf(2, 0) = 0.0; inf(2, 1) = 0.0; inf(2, 2) = 1.0;
    inf(3, 3) = 1E8; inf(3, 4) = 0.0; inf(3, 5) = 0.0;
    inf(4, 3) = 0.0; inf(4, 4) = 1E8; inf(4, 5) = 0.0;
    inf(5, 3) = 0.0; inf(5, 4) = 0.0; inf(5, 5) = 1E8;

    Eigen::Isometry3d t;
    double deg2rad = 3.14159265358979323846 / 180.0;
    Eigen::Matrix3d rotrpy = g2o::internal::fromEuler(Eigen::Vector3d(-58.0118418734 * deg2rad, -4.2868610819 * deg2rad, 27.3365702914 * deg2rad));//which 
    t = rotrpy;
    t.translation() = Eigen::Vector3d(-2848321.07828, 4660858.32744, 3282028.80025);
    g2o::VertexSE3* v = new g2o::VertexSE3;
    v->setId(0);
    v->setEstimate(t);
    optimizer.addVertex(v);

    size_t m_unaryEdgeNum = 0;
    g2o::EdgeSE3Prior* e = new g2o::EdgeSE3Prior;
    e->setVertex(0, v);
    e->setId(m_unaryEdgeNum++);
    e->setParameterId(0, 0);
    e->setMeasurement(t);
    e->setInformation(inf);
    optimizer.addEdge(e);

    Eigen::Isometry3d t_new = t;
    t_new.translation() = t.translation() - Eigen::Vector3d(10.0, 10.0, -5.0);
    Eigen::Matrix<double, 6, 6> inf_new = Eigen::Matrix<double, 6, 6>::Zero();
    Eigen::Matrix3d pos_inf_new = Eigen::Matrix3d::Identity();
    pos_inf_new(0, 0) = 1E8;
    pos_inf_new(1, 1) = 1E-8;
    pos_inf_new(2, 2) = 1E8;
    Eigen::Matrix3d  pos_cov_new = pos_inf_new.inverse();
    Eigen::Matrix3d  rot = t.rotation();
    Eigen::Matrix3d  rot_t = rot.transpose();
    Eigen::Matrix3d  ET = rot_t * pos_cov_new;
    Eigen::Matrix3d  pos_cov_trans = ET* rot;
    Eigen::Matrix3d  pos_inf_trans = pos_cov_trans.inverse();

    //Eigen::Matrix3d pos_inf_trans_llt = pos_cov_trans.llt().solve(Eigen::Matrix3d::Identity());
    Eigen::Matrix3d pos_inf_trans_llt = pos_cov_trans.ldlt().solve(Eigen::Matrix3d::Identity());
    std::cout << pos_inf_trans << std::endl;
    std::cout << "llt:" << std::endl;
    std::cout << pos_inf_trans_llt << std::endl;

    bool isSymmetric = pos_inf_trans_llt.transpose() == pos_inf_trans_llt;
    std::cout << isSymmetric << std::endl;
    inf_new.block<3, 3>(0, 0) = pos_inf_trans_llt;

    inf_new(3, 3) = 1E8; inf_new(3, 4) = 0.0; inf_new(3, 5) = 0.0;
    inf_new(4, 3) = 0.0; inf_new(4, 4) = 1E8; inf_new(4, 5) = 0.0;
    inf_new(5, 3) = 0.0; inf_new(5, 4) = 0.0; inf_new(5, 5) = 1E8;

    g2o::EdgeSE3Prior* e_new_z = new g2o::EdgeSE3Prior;
    e_new_z->setVertex(0, v);
    e_new_z->setId(m_unaryEdgeNum++);
    e_new_z->setParameterId(0, 0);
    e_new_z->setMeasurement(t_new);
    e_new_z->setInformation(inf_new);
    optimizer.addEdge(e_new_z);

    std::cout << std::fixed << std::setprecision(8) << inf_new << std::endl;

    bool psdFlag = optimizer.verifyInformationMatrices(true);
    if (!psdFlag)
    {
        std::cout << "no psd.\n";
    }
    optimizer.initializeOptimization();
    int iteration_count = optimizer.optimize(2000);
    std::cout << "finish" << std::endl;

注意由于计算器舍入误差，pos_inf_trans_llt通常不是对称矩阵，因此导致g2o验证正定失败，虽然可以跳过验证这一步，不影响优化结果，但是程序员不允许出现warning，因此通过把上三角赋值给下三角来解决：

    pos_inf_trans_llt(1, 0) = pos_inf_trans_llt(0, 1);
    pos_inf_trans_llt(2, 0) = pos_inf_trans_llt(0, 2);
    pos_inf_trans_llt(2, 1) = pos_inf_trans_llt(1, 2);