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

int 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
pixhawk 四旋翼筆記1

cpp 外部暴力訪問類內私有變量

opencv3.1+opencv_contrib 編譯出錯筆記

ubuntu indicator-multiload 配置

簡單座標變換 ch3 （7）

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結
