记录一个遇到的问题。
我的程序里只有两种类型的节点和一种类型的边。
节点: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