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 0int 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->setEstimate(landmarks_true_pose[i]);           // l->setEstimate(landmarks_true_pose[i] + Eigen::Vector3d((rand()%200-100)/5000.0,(rand()%200-100)/1000.0,(rand()%200-100)/1000.0));            if(i < 9)//第一帧九个点固定                l->setFixed(true);            l->setId(id++);            landmarks.push_back(l);            optimizer.addVertex(l);        }        for(int i=0; i<robot_true_pose.size()-1; i++)        {            double robot_x = robot_true_pose[i].translation()[0];            double robot_y = robot_true_pose[i].translation()[1];            for(int j=0; j<landmarks_true_pose.size(); j++)            {                double landmark_x = landmarks_true_pose[j][0];                double landmark_y = landmarks_true_pose[j][1];                if(sqrt(pow(robot_x-landmark_x,2) + pow(robot_y-landmark_y,2)) < 1.5)                {                    EdgeSE3PointXYZ* e = new EdgeSE3PointXYZ;                    e->setVertex(0, dynamic_cast<g2o::VertexSE3*> (vertices[i]));                    e->setVertex(1, dynamic_cast<g2o::VertexPointXYZ*> (landmarks[j]));                    e->setMeasurement(Eigen::Vector3d(landmark_x - robot_x,\                                                      landmark_y - robot_y,\                                                      2));                    e->setParameterId(0,0);//这句话必须加                    edges.push_back(e);                    optimizer.addEdge(e);                }            }        }        optimizer.save("test_near_before.g2o");        double sum_diff2=0;        for(int i=0; i<landmarks_true_pose.size(); i++)        {            g2o::HyperGraph::VertexIDMap::iterator v_it                = optimizer.vertices().find(robot_true_pose.size()+i);//optimizer.vertices()是一个map，可以根据索引号找到对应的vertex            VertexPointXYZ* vv = dynamic_cast< g2o::VertexPointXYZ * >(v_it->second);            Vector3d diff = vv->estimate()-landmarks_true_pose[i];            sum_diff2 += diff.dot(diff);        }        cout << "before BA " << sum_diff2 << endl;        optimizer.initializeOptimization();int 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->setEstimate(landmarks_true_pose[i]);               // l->setEstimate(landmarks_true_pose[i] + Eigen::Vector3d((rand()%200-100)/5000.0,(rand()%200-100)/1000.0,(rand()%200-100)/1000.0));                if(i < 9)//第一帧九个点固定                    l->setFixed(true);                l->setId(id++);                landmarks.push_back(l);                optimizer.addVertex(l);            }            for(int i=0; i<robot_true_pose.size()-1; i++)            {                double robot_x = robot_true_pose[i].translation()[0];                double robot_y = robot_true_pose[i].translation()[1];                for(int j=0; j<landmarks_true_pose.size(); j++)                {                    double landmark_x = landmarks_true_pose[j][0];                    double landmark_y = landmarks_true_pose[j][1];                    if(sqrt(pow(robot_x-landmark_x,2) + pow(robot_y-landmark_y,2)) < 1.5)                    {                        EdgeSE3PointXYZ* e = new EdgeSE3PointXYZ;                        e->setVertex(0, dynamic_cast<g2o::VertexSE3*> (vertices[i]));                        e->setVertex(1, dynamic_cast<g2o::VertexPointXYZ*> (landmarks[j]));                        e->setMeasurement(Eigen::Vector3d(landmark_x - robot_x,\                                                          landmark_y - robot_y,\                                                          2));                        e->setParameterId(0,0);//这句话必须加                        edges.push_back(e);                        optimizer.addEdge(e);                    }                }            }            optimizer.save("test_near_before.g2o");            double sum_diff2=0;            for(int i=0; i<landmarks_true_pose.size(); i++)            {                g2o::HyperGraph::VertexIDMap::iterator v_it                    = optimizer.vertices().find(robot_true_pose.size()+i);//optimizer.vertices()是一个map，可以根据索引号找到对应的vertex                VertexPointXYZ* vv = dynamic_cast< g2o::VertexPointXYZ * >(v_it->second);                Vector3d diff = vv->estimate()-landmarks_true_pose[i];                sum_diff2 += diff.dot(diff);            }            cout << "before BA " << sum_diff2 << endl;            optimizer.initializeOptimization();            optimizer.optimize(10);        /*            sum_diff2=0;            for(int i=0; i<landmarks_true_pose.size(); i++)            {                g2o::HyperGraph::VertexIDMap::iterator v_it                    = optimizer.vertices().find(robot_true_pose.size()+i);//optimizer.vertices()是一个map，可以根据索引号找到对应的vertex                VertexPointXYZ* vv = dynamic_cast< g2o::VertexPointXYZ * >(v_it->second);                Vector3d diff = vv->estimate()-landmarks_true_pose[i];                sum_diff2 += diff.dot(diff);            }            cout << "after BA " << sum_diff2 << endl;        */            optimizer.save("test_near_after.g2o");            return 0;        }        optimizer.optimize(10);    /*        sum_diff2=0;        for(int i=0; i<landmarks_true_pose.size(); i++)        {            g2o::HyperGraph::VertexIDMap::iterator v_it                = optimizer.vertices().find(robot_true_pose.size()+i);//optimizer.vertices()是一个map，可以根据索引号找到对应的vertex            VertexPointXYZ* vv = dynamic_cast< g2o::VertexPointXYZ * >(v_it->second);            Vector3d diff = vv->estimate()-landmarks_true_pose[i];            sum_diff2 += diff.dot(diff);        }        cout << "after BA " << sum_diff2 << endl;    */        optimizer.save("test_near_after.g2o");        return 0;    }    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->setEstimate(landmarks_true_pose[i]);       // l->setEstimate(landmarks_true_pose[i] + Eigen::Vector3d((rand()%200-100)/5000.0,(rand()%200-100)/1000.0,(rand()%200-100)/1000.0));        if(i < 9)//第一帧九个点固定            l->setFixed(true);        l->setId(id++);        landmarks.push_back(l);        optimizer.addVertex(l);    }    for(int i=0; i<robot_true_pose.size()-1; i++)    {        double robot_x = robot_true_pose[i].translation()[0];        double robot_y = robot_true_pose[i].translation()[1];        for(int j=0; j<landmarks_true_pose.size(); j++)        {            double landmark_x = landmarks_true_pose[j][0];            double landmark_y = landmarks_true_pose[j][1];            if(sqrt(pow(robot_x-landmark_x,2) + pow(robot_y-landmark_y,2)) < 1.5)            {                EdgeSE3PointXYZ* e = new EdgeSE3PointXYZ;                e->setVertex(0, dynamic_cast<g2o::VertexSE3*> (vertices[i]));                e->setVertex(1, dynamic_cast<g2o::VertexPointXYZ*> (landmarks[j]));                e->setMeasurement(Eigen::Vector3d(landmark_x - robot_x,\                                                  landmark_y - robot_y,\                                                  2));                e->setParameterId(0,0);//这句话必须加                edges.push_back(e);                optimizer.addEdge(e);            }        }    }    optimizer.save("test_near_before.g2o");    double sum_diff2=0;    for(int i=0; i<landmarks_true_pose.size(); i++)    {        g2o::HyperGraph::VertexIDMap::iterator v_it            = optimizer.vertices().find(robot_true_pose.size()+i);//optimizer.vertices()是一个map，可以根据索引号找到对应的vertex        VertexPointXYZ* vv = dynamic_cast< g2o::VertexPointXYZ * >(v_it->second);        Vector3d diff = vv->estimate()-landmarks_true_pose[i];        sum_diff2 += diff.dot(diff);    }    cout << "before BA " << sum_diff2 << endl;    optimizer.initializeOptimization();    optimizer.optimize(10);/*    sum_diff2=0;    for(int i=0; i<landmarks_true_pose.size(); i++)    {        g2o::HyperGraph::VertexIDMap::iterator v_it            = optimizer.vertices().find(robot_true_pose.size()+i);//optimizer.vertices()是一个map，可以根据索引号找到对应的vertex        VertexPointXYZ* vv = dynamic_cast< g2o::VertexPointXYZ * >(v_it->second);        Vector3d diff = vv->estimate()-landmarks_true_pose[i];        sum_diff2 += diff.dot(diff);    }    cout << "after BA " << sum_diff2 << endl;*/    optimizer.save("test_near_after.g2o");    return 0;}#elseint main(){    g2o::SparseOptimizer optimizer;//全局优化器    //以下三句话要加    g2o::ParameterSE3Offset* cameraOffset = new g2o::ParameterSE3Offset;    cameraOffset->setId(0);    optimizer.addParameter(cameraOffset);    optimizer.setVerbose(true);//调试信息输出    g2o::BlockSolverX::LinearSolverType * linearSolver;//6代表pose的自由度 3代表landmark的自由度    linearSolver= new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();    g2o::BlockSolverX * solver_ptr        = new g2o::BlockSolverX(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)/500.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->setEstimate(landmarks_true_pose[i]);        //l->setEstimate(landmarks_true_pose[i] + Eigen::Vector3d((rand()%200-100)/1000.0,(rand()%200-100)/1000.0,(rand()%200-100)/1000.0));        if(i < 9)//第一帧九个点固定            l->setFixed(true);        l->setId(id++);        landmarks.push_back(l);        optimizer.addVertex(l);    }    for(int i=0; i<robot_true_pose.size()-1; i++)    {        double robot_x = robot_true_pose[i].translation()[0];        double robot_y = robot_true_pose[i].translation()[1];        for(int j=0; j<landmarks_true_pose.size(); j++)        {            double landmark_x = landmarks_true_pose[j][0];            double landmark_y = landmarks_true_pose[j][1];            if(sqrt(pow(robot_x-landmark_x,2) + pow(robot_y-landmark_y,2)) < 1.5)            {                EdgeSE3PointXYZ* e = new EdgeSE3PointXYZ;                e->setVertex(0, dynamic_cast<g2o::VertexSE3*> (vertices[i]));                e->setVertex(1, dynamic_cast<g2o::VertexPointXYZ*> (landmarks[j]));                e->setMeasurement(Eigen::Vector3d(landmark_x - robot_x,\                                                  landmark_y - robot_y,\                                                  2));                e->setParameterId(0,0);//这句话必须加                edges.push_back(e);                optimizer.addEdge(e);            }        }    }    optimizer.save("test_near_before.g2o");    double sum_diff2=0;    for(int i=0; i<landmarks_true_pose.size(); i++)    {        g2o::HyperGraph::VertexIDMap::iterator v_it            = optimizer.vertices().find(robot_true_pose.size()+i);//optimizer.vertices()是一个map，可以根据索引号找到对应的vertex        VertexPointXYZ* vv = dynamic_cast< g2o::VertexPointXYZ * >(v_it->second);        Vector3d diff = vv->estimate()-landmarks_true_pose[i];        sum_diff2 += sqrt(diff.dot(diff));    }    cout << "before BA " << sum_diff2 << endl;    optimizer.initializeOptimization();    optimizer.optimize(10);    sum_diff2=0;    for(int i=0; i<landmarks_true_pose.size(); i++)    {        g2o::HyperGraph::VertexIDMap::iterator v_it            = optimizer.vertices().find(robot_true_pose.size()+i);//optimizer.vertices()是一个map，可以根据索引号找到对应的vertex        VertexPointXYZ* vv = dynamic_cast< g2o::VertexPointXYZ * >(v_it->second);        Vector3d diff = vv->estimate()-landmarks_true_pose[i];        sum_diff2 += sqrt(diff.dot(diff));    }    sum_diff2 /= landmarks_true_pose.size();    cout << "after BA " << sum_diff2 << endl;    optimizer.save("test_near_after.g2o");    return 0;}#endif