激光SLAM理论与实践 第六次作业（G2o 优化方法）

1. 补充代码，实现高斯牛顿方法对 Pose-Graph 进行优化；

答：计算雅可比：

  Eigen::Matrix2d Ri,Ri_t,Rj,Rj_t,Rij ,Rij_t ,d_Ri_T,e1;
    Eigen::Vector2d ti,tj,tij,e2;
    Eigen::Matrix3d Ti = PosetoTran(xi);
    Eigen::Matrix3d Tj = PosetoTran(xj);
    Eigen::Matrix3d Tij = Ti.transpose()*Tj;
    Ri  = Ti.block(0,0,2,2);
    Rj = Tj.block(0,0,2,2);
    Rij = Tij.block(0,0,2,2);
    Ri_t = Ti.transpose().block(0,0,2,2);
    Rj_t = Tj.transpose().block(0,0,2,2);
    Rij_t = Tij.transpose().block(0,0,2,2);      
    ti =Eigen::Vector2d(xi(0),xi(1));
    tj =Eigen::Vector2d(xj(0),xj(1));
    tij=Eigen::Vector2d(z(0),z(1));
    d_Ri_T = Ti.transpose().block(0,0,2,2);
    Eigen::Vector2d error = Rij_t*(Ri_t*(tj- ti)-tij);
    double col = GN_NormalizationAngles(xj(2)-xi(2)-z(2));
    ei  = Eigen::Vector3d(error(0),error(1),col);
    e1 =  Rij_t*Ri_t;
    e2 =  Rij_t* d_Ri_T*( tj- ti);
    Ai.block(0,0,2,2) = -e1;
    Ai.block(0,2,2,1) = e2;
    Ai.block(2,0,1,3) = Eigen::Vector3d(0.0,0.0,-1.0).transpose();
    Bi.setIdentity();
    Bi.block(0,0,2,2) = e1;

计算：H和b矩阵

H.block(3*tmpEdge.xi,3*tmpEdge.xi,3,3) += Ai.transpose()*infoMatrix*Ai;
H.block(3*tmpEdge.xi,3*tmpEdge.xj,3,3) += Ai.transpose()*infoMatrix*Bi;
H.block(3*tmpEdge.xj,3*tmpEdge.xi,3,3) += Bi.transpose()*infoMatrix*Ai;
H.block(3*tmpEdge.xj,3*tmpEdge.xj,3,3) += Bi.transpose()*infoMatrix*Bi;
b.block(3*tmpEdge.xi,0,3,1) += Ai.transpose()*infoMatrix*ei;
b.block(3*tmpEdge.xj,0,3,1) += Bi.transpose()*infoMatrix*ei;

计算dx，利用最小二乘
  dx = (H.transpose()*H).llt().solve(H.transpose()*(-b));
更新节点，因为dx三个元素=一个顶点
for(int j = 0;j<Vertexs.size();j++)
  Vertexs[j] += Eigen::Vector3d(dx(3*j),dx(3*j+1),dx(3*j+2));

2. 简答题，开放性答案：你认为第一题的优化过程中哪个环节耗时最多？是否有什么改进的方法可以进行加速？

答：主要在将计算好的多维度H和b矩阵通过最小二乘理论计算dx变化量的值耗时最多。可以通过使用ceres、g2o等优化库对H简化，加速求解dx，比如g2oSLAM里，H矩阵往往有特定的稀疏形式，为实时求解优化问题提供了可能性。利用稀疏形式的消元，分解，最后再求解增量，会让求解效率大大提高。

3. 学习相关材料。除了高斯牛顿方法，你还知道哪些非线性优化方法？请简述它们的原理；

3. 将第一题改为使用任意一种非线性优化库进行优化（比如 Ceres, Gtsam 或 G2o 等）。
   答：main函数去掉高斯牛顿函数的迭代部分，加入g2o函数入口

   G2oOptimize(  Vertexs, Edges, maxIteration))；

加入创建优化器和进行优化部分函数

定义边：

定义顶点：

实验结果：

详细代码

main.cpp

#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>
#include <gaussian_newton.h>
#include <readfile.h>
#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.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 <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/factory.h>
#include <g2o/core/hyper_graph.h>

#include <Eigen/Core>
#include <cmath>
const double GN_PI = 3.1415926;
double GN_NormalizationAngle(double angle)
{
    
    
    if(angle > GN_PI)
        angle -= 2*GN_PI;
    else if(angle < -GN_PI)
        angle += 2*GN_PI;

    return angle;
}

Eigen::Matrix3d PosetoTrans(Eigen::Vector3d x)
{
    
    
    Eigen::Matrix3d T ;
    T << cos(x(2)),-sin(x(2)),x(0),
         sin(x(2)),cos(x(2)),x(1),
         0.0,0.0,1;
         return T;
}
// 定义定点
class PoseVertex: public g2o::BaseVertex<3,Eigen::Vector3d>
{
    
    
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     PoseVertex(){
    
    }
    PoseVertex( Eigen::Vector3d org ){
    
    
        _org = org;
    }
    virtual void setToOriginImpl()
    {
    
    
        _estimate = _org;
    }
    virtual void oplusImpl(const double* update)
    {
    
    
        _estimate +=Eigen::Vector3d(update);
        _estimate(2) = GN_NormalizationAngle(_estimate(2));
    }
    virtual bool read(std::istream& in) {
    
    }//return true;
    virtual bool write(std::ostream& out) const {
    
    }
private:
   Eigen::Vector3d _org;   
};

 //维度    类型         顶点类型
class  PoseEdge: public g2o::BaseBinaryEdge<3, Eigen::Vector3d  ,PoseVertex  ,PoseVertex>
{
    
    
public:
          EIGEN_MAKE_ALIGNED_OPERATOR_NEW  
          //放在public 下
        // 在生成定长的Matrix或Vector对象时，需要开辟内存，调用默认构造函数，
        // 通常x86下的指针是32位，内存位数没对齐就会导致程序运行出错。
        // 而对于动态变量(例如Eigen::VectorXd)会动态分配内存，因此会自动地进行内存对齐。
         PoseEdge(){
    
    }
         void computeError()
         {
    
    
             const PoseVertex* v0 = static_cast<const PoseVertex*>(_vertices[0]);
             const PoseVertex* v1 = static_cast<const PoseVertex*>(_vertices[1]);
             const Eigen::Vector3d xi = v0->estimate();
             const Eigen::Vector3d xj = v1->estimate();
             const Eigen::Vector3d z = _measurement;
            Eigen::Matrix3d Ti = PosetoTrans(xi);
            Eigen::Matrix3d Tj = PosetoTrans(xj);
            Eigen::Matrix3d Tij = Ti.transpose()*Tj;
            _Ri  = Ti.block(0,0,2,2);
            _Rj = Tj.block(0,0,2,2);
            _Rij = Tij.block(0,0,2,2);
            _Ri_t = Ti.transpose().block(0,0,2,2);
            _Rj_t = Tj.transpose().block(0,0,2,2);
            _Rij_t = Tij.transpose().block(0,0,2,2);
            _ti =Eigen::Vector2d(xi(0),xi(1));
            _tj =Eigen::Vector2d(xj(0),xj(1));
            _tij=Eigen::Vector2d(z(0),z(1));
            _d_Ri_T = Ti.transpose().block(0,0,2,2);
            //  _Ri << cos(xi(2)),-sin(xi(2)),
            //         sin(xi(2)),cos(xi(2));
            //  _Rj << cos(xj(2)),-sin(xj(2)),
            //         sin(xj(2)),cos(xj(2));
            //  _Rij << cos(z(2)),-sin(z(2)),
            //         sin(z(2)),cos(z(2));  
            // _detc_Ri_T <<   -sin(xi(2)),cos(xi(2)),
            //        -cos(xi(2)),-sin(xi(2)); 
            //  _ti << xi(0),xi(1);
            //  _tj << xj(0),xj(1);
            //  _tij << z(0),z(1);
            Eigen::Vector2d error = _Rij_t*(_Ri_t*(_tj-_ti)-_tij);
            double col =GN_NormalizationAngle(xj(2)-xi(2)-z(2));
            _error  = Eigen::Vector3d(error(0),error(1),col);
         }
          void linearizeOplus()   
          {
    
    
            _e1 = _Rij_t*_Ri_t;
            _e2 = _Rij_t*_d_Ri_T*(_tj-_ti);
            // _jacobianOplusXi.block(0,0,3,3) = -Eigen::Matrix3d().setIdentity(); 
            // _jacobianOplusXi.block(0,0,2,2) = _e1.block(0,0,2,2);
            // _jacobianOplusXi.block(0,2,2,1) = _e2.block(0,0,2,1);
            // _jacobianOplusXj.block(0,0,3,3) = Eigen::Matrix3d().setIdentity(); 
            // _jacobianOplusXj.block(0,0,2,2) = _e1.block(0,0,2,2);
            _jacobianOplusXi.block(0,0,2,2) = -_e1;//-_Rij_t*_Ri_t;
            _jacobianOplusXi.block(0,2,2,1) = _e2;
            _jacobianOplusXi.block(2,0,1,3)  = Eigen::Vector3d(0.0,0.0,-1.0).transpose();
            _jacobianOplusXj.block(0,0,2,2) = _e1;
            _jacobianOplusXj.block(0,2,2,1) =Eigen::Vector2d(0.0,0.0);
            _jacobianOplusXj.block(2,0,1,3) = Eigen::Vector3d(0.0,0.0,1.0).transpose();
          }
          virtual bool read(std::istream& in)   {
    
    }
          virtual bool write(std::ostream& out) const {
    
    }
private:
         Eigen::Matrix2d _Ri ,_Ri_t,_Rj,_Rj_t,_Rij ,_Rij_t ,_d_Ri_T,_e1;
         Eigen::Vector2d _ti,_tj,_tij,_e2;
};
//for visual
void PublishGraphForVisulization(ros::Publisher* pub,
                                 std::vector<Eigen::Vector3d>& Vertexs,
                                 std::vector<Edge>& Edges,
                                 int color = 0)
{
    
    
    visualization_msgs::MarkerArray marray;

    //point--red
    visualization_msgs::Marker m;
    m.header.frame_id = "map";
    m.header.stamp = ros::Time::now();
    m.id = 0;
    m.ns = "ls-slam";
    m.type = visualization_msgs::Marker::SPHERE;
    m.pose.position.x = 0.0;
    m.pose.position.y = 0.0;
    m.pose.position.z = 0.0;
    m.scale.x = 0.1;
    m.scale.y = 0.1;
    m.scale.z = 0.1;
    if(color == 0)
    {
    
    
        m.color.r = 1.0;
        m.color.g = 0.0;
        m.color.b = 0.0;
    }
    else
    {
    
    
        m.color.r = 0.0;
        m.color.g = 1.0;
        m.color.b = 0.0;
    }
    m.color.a = 1.0;
    m.lifetime = ros::Duration(0);
    //linear--blue
    visualization_msgs::Marker edge;
    edge.header.frame_id = "map";
    edge.header.stamp = ros::Time::now();
    edge.action = visualization_msgs::Marker::ADD;
    edge.ns = "karto";
    edge.id = 0;
    edge.type = visualization_msgs::Marker::LINE_STRIP;
    edge.scale.x = 0.1;
    edge.scale.y = 0.1;
    edge.scale.z = 0.1;
    if(color == 0)
    {
    
    
        edge.color.r = 0.0;
        edge.color.g = 0.0;
        edge.color.b = 1.0;
    }
    else
    {
    
    
        edge.color.r = 1.0;
        edge.color.g = 0.0;
        edge.color.b = 1.0;
    }
    edge.color.a = 1.0;
    m.action = visualization_msgs::Marker::ADD;
    uint id = 0;
    //加入节点
    for (uint i=0; i<Vertexs.size(); i++)
    {
    
    
        m.id = id;
        m.pose.position.x = Vertexs[i](0);
        m.pose.position.y = Vertexs[i](1);
        marray.markers.push_back(visualization_msgs::Marker(m));
        id++;
    }

    //加入边
    for(int i = 0; i < Edges.size();i++)
    {
    
    
        Edge tmpEdge = Edges[i];
        edge.points.clear();
        geometry_msgs::Point p;
        p.x = Vertexs[tmpEdge.xi](0);
        p.y = Vertexs[tmpEdge.xi](1);
        edge.points.push_back(p);
        p.x = Vertexs[tmpEdge.xj](0);
        p.y = Vertexs[tmpEdge.xj](1);
        edge.points.push_back(p);
        edge.id = id;

        marray.markers.push_back(visualization_msgs::Marker(edge));
        id++;
    }
    pub->publish(marray);
}
 bool G2oOptimize(std::vector<Eigen::Vector3d>& Vertexs,
                  std::vector<Edge>& Edges,const int maxIteration)
{
    
    
        typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,3> >Block;
    Block::LinearSolverType* linearSolverType = new g2o::LinearSolverEigen<Block::PoseMatrixType>();
    Block* solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linearSolverType));
    // std::unique_ptr<Block::LinearSolverType> linearSolverType ( new g2o::LinearSolverCSparse<Block::PoseMatrixType>());
    // std::unique_ptr<Block> solver_ptr ( new Block ( std::move(linearSolverType)));  
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr));
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);//打开调试输出
    std::vector< PoseVertex* > vertice;
    for(int i = 0;i<Vertexs.size();i++)
    {
    
    
        PoseVertex* v=new PoseVertex(Vertexs[i]) ;//Vertexs[i]
        if(i == 0)
        v->setFixed(true);
        v->setEstimate(Vertexs[i]);
        v->setId(i);
        optimizer.addVertex(v);
        vertice.push_back(v);
    }
    for(int i= 0;i<Edges.size();i++)
    {
    
    
        PoseEdge* e = new PoseEdge();
        e->setId(i);
        e->setVertex(0,vertice[Edges[i].xi]);
        e->setVertex(1,vertice[Edges[i].xj]);
        e->setMeasurement(Edges[i].measurement);
        e->setInformation(Edges[i].infoMatrix);
        optimizer.addEdge(e);
    }
    optimizer.initializeOptimization();
    optimizer.optimize(maxIteration);

    for(int i =0;i<vertice.size();i++)
       Vertexs[i] = vertice[i]->estimate();
    std::cout <<"finish" <<std::endl;  
    return true;
}
int main(int argc, char **argv)
{
    
    
    ros::init(argc, argv, "ls_slam");
    ros::NodeHandle nodeHandle;
    // beforeGraph
    ros::Publisher beforeGraphPub,afterGraphPub;
    beforeGraphPub = nodeHandle.advertise<visualization_msgs::MarkerArray>("beforePoseGraph",1,true);
    afterGraphPub  = nodeHandle.advertise<visualization_msgs::MarkerArray>("afterPoseGraph",1,true);
    // std::string VertexPath = "/home/lcp/ws/shenlan2d_ws/src/ls_slam/data/test_quadrat-v.dat";
    // std::string EdgePath = "/home/lcp/ws/shenlan2d_ws/src/ls_slam/data/test_quadrat-e.dat";
 
   std::string VertexPath = "/home/lcp/ws/shenlan2d_ws/src/ls_slam/data/intel-v.dat";
   std::string EdgePath = "/home/lcp/ws/shenlan2d_ws/src/ls_slam/data/intel-e.dat";
//    std::string VertexPath = "/home/lcp/ws/shenlan2d_ws/src/ls_slam/data/killian-v.dat";
//     std::string EdgePath = "/home/lcp/ws/shenlan2d_ws/src/ls_slam/data/killian-e.dat";

    std::vector<Eigen::Vector3d> Vertexs;
    std::vector<Edge> Edges;
    //从路径读取文件存到 Vertexs 和Edges
    ReadVertexInformation(VertexPath,Vertexs);
    ReadEdgesInformation(EdgePath,Edges);
    PublishGraphForVisulization(&beforeGraphPub,
                                Vertexs,
                                Edges);
    double initError = ComputeError(Vertexs,Edges);
    std::cout <<"initError:"<<initError<<std::endl;
    int maxIteration = 100;
    double epsilon = 1e-4;
    #if 1
    for(int i = 0; i < maxIteration;i++)
    {
    
    
        std::cout <<"Iterations:"<<i<<std::endl;
        Eigen::VectorXd dx = LinearizeAndSolve(Vertexs,Edges);
        //进行更新
        //TODO--Start
        std::cout <<"更新值----------------- " <<std::endl;
        for(int j = 0;j<Vertexs.size();j++)
        {
    
    
            Vertexs[j] += Eigen::Vector3d(dx(3*j),dx(3*j+1),dx(3*j+2));
        }
        //TODO--End
        double maxError = -1;
        for(int k = 0; k < 3 * Vertexs.size();k++)
        {
    
    
            if(maxError < std::fabs(dx(k)))
            {
    
    
                maxError = std::fabs(dx(k));
            }
        }
        if(maxError < epsilon)
            break;
    }
    #else
    if(! G2oOptimize(  Vertexs, Edges, maxIteration))
    
    {
    
    
         std::cout <<"优化出错" <<std::endl;
         return 0;}
    #endif 
    double finalError  = ComputeError(Vertexs,Edges);
    std::cout <<"FinalError:"<<finalError<<std::endl;
    PublishGraphForVisulization(&afterGraphPub,
                                Vertexs,
                                Edges,1);
    ros::spin();
    return 0;
}

gaussian_newton.cpp

#include "gaussian_newton.h"
#include <eigen3/Eigen/Jacobi>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Householder>
#include <eigen3/Eigen/Cholesky>
#include <eigen3/Eigen/LU>

#include <iostream>

const double GN_PI = 3.1415926;

//进行角度正则化．
double GN_NormalizationAngles(double angle)
{
    
    
    if(angle > GN_PI)
        angle -= 2*GN_PI;
    else if(angle < -GN_PI)
        angle += 2*GN_PI;
    return angle;
}
Eigen::Matrix3d PosetoTran(Eigen::Vector3d x)
{
    
    
    Eigen::Matrix3d T ;
    T << cos(x(2)),-sin(x(2)),x(0),
         sin(x(2)),cos(x(2)),x(1),
         0.0,0.0,1;
         return T;
}
//位姿-->转换矩阵
Eigen::Matrix3d PoseToTrans(Eigen::Vector3d x)
{
    
    
    Eigen::Matrix3d trans;
    trans << cos(x(2)),-sin(x(2)),x(0),
             sin(x(2)), cos(x(2)),x(1),
                     0,         0,    1;
    return trans;
}
//转换矩阵－－＞位姿
Eigen::Vector3d TransToPose(Eigen::Matrix3d trans)
{
    
    
    Eigen::Vector3d pose;
    pose(0) = trans(0,2);
    pose(1) = trans(1,2);
    pose(2) = atan2(trans(1,0),trans(0,0));
    return pose;
}
//计算整个pose-graph的误差
double ComputeError(std::vector<Eigen::Vector3d>& Vertexs,
                    std::vector<Edge>& Edges)
{
    
    
    double sumError = 0;
    for(int i = 0; i < Edges.size();i++)
    {
    
    
        Edge tmpEdge = Edges[i];
        Eigen::Vector3d xi = Vertexs[tmpEdge.xi];
        Eigen::Vector3d xj = Vertexs[tmpEdge.xj];
        Eigen::Vector3d z = tmpEdge.measurement;
        Eigen::Matrix3d infoMatrix = tmpEdge.infoMatrix;
        Eigen::Matrix3d Xi = PosetoTran(xi);
        Eigen::Matrix3d Xj = PosetoTran(xj);
        Eigen::Matrix3d Z  = PosetoTran(z);
        Eigen::Matrix3d Ei = Z.inverse() *  Xi.inverse() * Xj;
        Eigen::Vector3d ei = TransToPose(Ei);
        sumError += ei.transpose() * infoMatrix * ei;
    }
    return sumError;
}
/**
 * @brief CalcJacobianAndError
 *         计算jacobian矩阵和error
 * @param xi    fromIdx
 * @param xj    toIdx
 * @param z     观测值:xj相对于xi的坐标
 * @param ei    计算的误差
 * @param Ai    相对于xi的Jacobian矩阵
 * @param Bi    相对于xj的Jacobian矩阵
 */
void CalcJacobianAndError(Eigen::Vector3d xi,Eigen::Vector3d xj,Eigen::Vector3d z,
                          Eigen::Vector3d& ei,Eigen::Matrix3d& Ai,Eigen::Matrix3d& Bi)
{
    
    
    //TODO--Start
      Eigen::Matrix2d Ri,Ri_t,Rj,Rj_t,Rij ,Rij_t ,d_Ri_T,e1;
      Eigen::Vector2d ti,tj,tij,e2;
    Eigen::Matrix3d Ti = PosetoTran(xi);
    Eigen::Matrix3d Tj = PosetoTran(xj);
    Eigen::Matrix3d Tij = Ti.transpose()*Tj;
    Ri  = Ti.block(0,0,2,2);
    Rj = Tj.block(0,0,2,2);
    Rij = Tij.block(0,0,2,2);
    Ri_t = Ti.transpose().block(0,0,2,2);
    Rj_t = Tj.transpose().block(0,0,2,2);
    Rij_t = Tij.transpose().block(0,0,2,2);      
    ti =Eigen::Vector2d(xi(0),xi(1));
    tj =Eigen::Vector2d(xj(0),xj(1));
    tij=Eigen::Vector2d(z(0),z(1));
    d_Ri_T = Ti.transpose().block(0,0,2,2);
    Eigen::Vector2d error = Rij_t*(Ri_t*(tj- ti)-tij);
    double col = GN_NormalizationAngles(xj(2)-xi(2)-z(2));
    ei  = Eigen::Vector3d(error(0),error(1),col);
    e1 =  Rij_t*Ri_t;
    e2 =  Rij_t* d_Ri_T*( tj- ti);

    // Ri << cos(xi(2)),-sin(xi(2)),
    //       sin(xi(2)),cos(xi(2));
    // Rj << cos(xj(2)),-sin(xj(2)),
    //       sin(xj(2)),cos(xj(2));
    // Rij << cos(z(2)),-sin(z(2)),
    //       sin(z(2)),cos(z(2));  
    // //先转置再求导数
    // detc_Ri_T <<   -sin(xi(2)),cos(xi(2)),
    //                -cos(xi(2)),-sin(xi(2));    
    // Eigen::Vector2d ti,tj,tij,e2;
    //  ti << xi(0),xi(1);
    //  tj << xj(0),xj(1);
    //  tij << z(0),z(1);
    // ei.block(0,0,2,1) = Rij.transpose()*(Ri.transpose()*(tj-ti)-tij);
    // ei(2) = GN_NormalizationAngles(xj(2)-xi(2)-z(2));//角度归一化
    // e1 = -Rij.transpose()*Ri.transpose();
    // e2 = Rij.transpose()*detc_Ri_T*(tj-ti);
    // Ai = -Ai.setIdentity();
    Ai.block(0,0,2,2) = -e1;
    Ai.block(0,2,2,1) = e2;
    Ai.block(2,0,1,3) = Eigen::Vector3d(0.0,0.0,-1.0).transpose();
    Bi.setIdentity();
    Bi.block(0,0,2,2) = e1;
//    Ai << e1(0,0),e1(0,1),e2(0),
//          e1(1,0),e1(1,1),e2(1),
//           0.0, 0.0, -1.0;
    //TODO--end
}
/**
 * @brief LinearizeAndSolve
 *        高斯牛顿方法的一次迭代．
 * @param Vertexs   图中的所有节点
 * @param Edges     图中的所有边
 * @return          位姿的增量
 */
Eigen::VectorXd  LinearizeAndSolve(std::vector<Eigen::Vector3d>& Vertexs,
                                   std::vector<Edge>& Edges)
{
    
    
    //申请内存
    Eigen::MatrixXd H(Vertexs.size() * 3,Vertexs.size() * 3);
    Eigen::VectorXd b(Vertexs.size() * 3);
    H.setZero();
    b.setZero();
    //固定第一帧
    Eigen::Matrix3d I;
    I.setIdentity();
    H.block(0,0,3,3) += I;
    //构造H矩阵　＆ b向量
    // std::cout <<"计算H和B的值++++++++++++++++"<<std::endl;
    for(int i = 0; i < Edges.size();i++)
    {
    
    
        //提取信息
        Edge tmpEdge = Edges[i];
        Eigen::Vector3d xi = Vertexs[tmpEdge.xi];
        Eigen::Vector3d xj = Vertexs[tmpEdge.xj];
        Eigen::Vector3d z = tmpEdge.measurement;
        Eigen::Matrix3d infoMatrix = tmpEdge.infoMatrix;

        //计算误差和对应的Jacobian
        Eigen::Vector3d ei;
        Eigen::Matrix3d Ai;
        Eigen::Matrix3d Bi;
        // std::cout <<"计算雅可比值JJJJJJJJJJJJJJJJJJ"<<std::endl;
        CalcJacobianAndError(xi,xj,z,ei,Ai,Bi);
        // std::cout <<"计算雅可比值JJJJJJJJJJJJJJJJJJ完毕"<<std::endl;
         //TODO--Start
       H.block(3*tmpEdge.xi,3*tmpEdge.xi,3,3) += Ai.transpose()*infoMatrix*Ai;
       H.block(3*tmpEdge.xi,3*tmpEdge.xj,3,3) += Ai.transpose()*infoMatrix*Bi;
       H.block(3*tmpEdge.xj,3*tmpEdge.xi,3,3) += Bi.transpose()*infoMatrix*Ai;
       H.block(3*tmpEdge.xj,3*tmpEdge.xj,3,3) += Bi.transpose()*infoMatrix*Bi;
       b.block(3*tmpEdge.xi,0,3,1) += Ai.transpose()*infoMatrix*ei;
       b.block(3*tmpEdge.xj,0,3,1) += Bi.transpose()*infoMatrix*ei;
        //TODO--End
    }
    //  std::cout <<"计算H和B的值++++++++++++++++完毕"<<std::endl;
    //求解
    Eigen::VectorXd dx(Vertexs.size()*3);
    //TODO--Start  
    // Eigen::MatrixXd ATA = H.transpose()*H;
    // Eigen::VectorXd ATb = H.transpose()*(-b);
    //最小二乘
    //(A.transpose()*A).inverse()*A.transpose()*b; 
    //   dx = -ATA.inverse()*ATb;
    // std::cout <<"最小二乘求解****************"<<std::endl;
    // dx = ATA.llt().solve(ATb);
    dx = (H.transpose()*H).llt().solve(H.transpose()*(-b));
    // dx.setZero();
    // typedef Eigen::SparseMatrixBase<double> spMat;
    // spMat A = H.sparseView();
    // Eigen::Solv<spMat> solver;
    // solver.compute(A);
    // if(solver.info() == Eigen::Success)
    // dx = solver.solve(-b);
    // std::cout <<"最小二乘求解****************完毕"<<std::endl;
     {
    
    
        //   std::cout <<"elseXXXXXXXXXXXXXXXX"<<std::endl;
        // dx = ATA.lu().solve(ATb);
        // dx = ATA.llt().solve(ATb);
        // dx = H.colPivHouseholderQr().solve(b);
        // dx = H.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV ).solve(b);

        // Eigen::JacobiSVD<Eigen::MatrixXd> svd(H,Eigen::ComputeFullU);
        // Eigen::VectorXd evals = svd.singularValues();
        // Eigen::MatrixXd evecs = svd.matrixU();
        // Eigen::MatrixXd lambds(evals.size(),evals.size());
        // lambds.setIdentity();
        // for(int i ; i<evals.size();i++)
        // {
    
    
        //     lambds(i,i)=1.0/evals(i);
        // }
        // dx = -1*(evecs*(lambds*(evecs.transpose()*b)));
     }  
    //TODO-End
    return dx;
}