在给自定义的边添加顶点时,触发以下错误:
void setVertex(size_t i, Vertex* v) {
assert(i < _vertices.size() && "index out of bounds");
_vertices[i] = v;
}
直观理解就是vector容器越界,因为没有给它进行resize();
解决方法:
我参考了g2o\types\slam3d_addons\edge_se3_calib.cpp里的多元边类的实现方法,发现需要在该类的构造函数中,调用resize()函数,三元边就是resize(3),即可解决。
参考如下:
EdgeSE3Calib::EdgeSE3Calib() : BaseVariableSizedEdge<6, Isometry3>() {
resize(3);
}
void EdgeSE3Calib::computeError() {
const VertexSE3* v1 = static_cast<const VertexSE3*>(_vertices[0]);
const VertexSE3* v2 = static_cast<const VertexSE3*>(_vertices[1]);
const VertexSE3* calib = static_cast<const VertexSE3*>(_vertices[2]);
_error = g2o::internal::toVectorMQT(
_measurement.inverse() * calib->estimate().inverse() *
v1->estimate().inverse() * v2->estimate() * calib->estimate());
}