g2o 简单测试1

记录一个遇到的问题。
我的程序里只有两种类型的节点和一种类型的边。
节点：VertexPointXYZ VertexSE3
边：EdgeSE3PointXYZ

当我使用 BlockSolver_6_3 这个solver时 一旦执行 optimizer.optimize(10); 程序就崩溃。
按理说 BlockSolver_6_3 的意思是 6代表pose的自由度 3代表landmark的自由度 应该和我定义的节点和边是一致的，但是为何会出错。会跳出一个eigen的维度不匹配的错误。我使用保存下来的数据，在g2o_viewer里调用同样的方法去求解，就不会出错。

之后只能把 BlockSolver_6_3 改为了 BlockSolverX 才解决这个问题。

下面是代码

#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <Eigen/Geometry>

#include "g2o/types/slam3d/types_slam3d.h"
#include "g2o/types/slam3d/vertex_pointxyz.h"
#include "g2o/types/slam3d/vertex_se3.h"
#include "g2o/types/slam3d/edge_se3.h"
#include "g2o/types/slam3d/edge_se3_pointxyz.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/core/optimization_algorithm_levenberg.h"



using namespace std;
using namespace g2o;
using namespace Eigen;
#if 0
int main()
{

    g2o::SparseOptimizer optimizeBlockSolver_6_3r;//全局优化器
    //以下三句话要加
    g2o::ParameterSE3Offset* cameraOffset = new g2o::ParameterSE3Offset;
    cameraOffset->setId(0);
    optimizer.addParameter(cameraOffset);

    optimizer.setVerbose(true);//调试信息输出
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;//6代表pose的自由度 3代表landmark的自由度
    linearSolver= new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();

    g2o::BlockSolver_6_3 * solver_ptr
        = new g2o::BlockSolver_6_3(linearSolver);
    //优化方法LMint main()
    {

        g2o::SparseOptimizer optimizer;//全局优化器
        //以下三句话要加
        g2o::ParameterSE3Offset* cameraOffset = new g2o::ParameterSE3Offset;
        cameraOffset->setId(0);
        optimizer.addParameter(cameraOffset);

        optimizer.setVerbose(true);//调试信息输出
        g2o::BlockSolver_6_3::LinearSolverType * linearSolver;//6代表pose的自由度 3代表landmark的自由度
        linearSolver= new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();

        g2o::BlockSolver_6_3 * solver_ptr
            = new g2o::BlockSolver_6_3(linearSolver);
        //优化方法LM
        g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
        optimizer.setAlgorithm(solver);

        vector<Eigen::Vector3d> landmarks_true_pose;
        vector<Eigen::Isometry3d> robot_virtual_pose;
        vector<Eigen::Isometry3d> robot_true_pose;

        vector<VertexPointXYZ*> landmarks;
        vector<VertexSE3*> vertices;//pose
        vector<EdgeSE3PointXYZ*> edges;

        //设置landmark真值
        for(int i=-1; i<20; i++)
            for(int j=-1; j<2; j++)
                landmarks_true_pose.push_back(Eigen::Vector3d(i, j, 2));
        //设置机器人运动轨迹
        for(int i=0; i<20; i++)
        {
            Eigen::Isometry3d T = Eigen::Isometry3d::Identity();//旋转和平移的集合,4*4的矩阵
            Eigen::Matrix3d rot = Eigen::Matrix3d::Identity();
            Eigen::Vector3d trans =  Eigen::Vector3d(i, 0, 0);
            T.rotate(rot);
            T.translate(trans);
            //cout << "Transform matrix = \n" << T.matrix() <<endl;
            robot_true_pose.push_back(T);

            T = Eigen::Isometry3d::Identity();
            trans =  Eigen::Vector3d(i + (rand()%200-100)/5000.0, 0, 0);
            //trans =  Eigen::Vector3d(i /2.0, 0, 0);
            T.translate(trans);
            robot_virtual_pose.push_back(T);
        }
        int id=0;
        for(int i=0; i<robot_true_pose.size(); i++)
        {
            VertexSE3* v = new VertexSE3;
           // v->setEstimate(robot_true_pose[i]);
            v->setEstimate(robot_virtual_pose[i]);
            v->setId(id++);
            vertices.push_back(v);
            optimizer.addVertex(v);

    /*
            g2o::HyperGraph::VertexIDMap::iterator v_it
                = optimizer.vertices().find(id-1);//optimizer.vertices()是一个map，可以根据索引号找到对应的vertex
            VertexSE3* vv = dynamic_cast< g2o::VertexSE3 * >(v_it->second);
            cout << "robot" << endl;
            cout << vv->estimate().translation() << endl;
            cout << v->estimate().translation() << endl;
    */

        }

        for(int i=0; i<landmarks_true_pose.size(); i++)
        {
            VertexPointXYZ* l = new VertexPointXYZ;
            l->setEstima