diff --git a/g2o/core/base_binary_edge.hpp b/g2o/core/base_binary_edge.hpp index 9d14f85ee..943fbcc05 100644 --- a/g2o/core/base_binary_edge.hpp +++ b/g2o/core/base_binary_edge.hpp @@ -131,8 +131,8 @@ void BaseBinaryEdge::constructQuadraticForm() template void BaseBinaryEdge::linearizeOplus(JacobianWorkspace& jacobianWorkspace) { - new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, Di); - new (&_jacobianOplusXj) JacobianXjOplusType(jacobianWorkspace.workspaceForVertex(1), D, Dj); + new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D < 0 ? _dimension : D, Di); + new (&_jacobianOplusXj) JacobianXjOplusType(jacobianWorkspace.workspaceForVertex(1), D < 0 ? _dimension : D, Dj); linearizeOplus(); } diff --git a/g2o/core/base_multi_edge.hpp b/g2o/core/base_multi_edge.hpp index 72c9e75a9..8c446ea2a 100644 --- a/g2o/core/base_multi_edge.hpp +++ b/g2o/core/base_multi_edge.hpp @@ -54,7 +54,7 @@ void BaseMultiEdge::linearizeOplus(JacobianWorkspace& jacobianWorkspace) for (size_t i = 0; i < _vertices.size(); ++i) { OptimizableGraph::Vertex* v = static_cast(_vertices[i]); assert(v->dimension() >= 0); - new (&_jacobianOplus[i]) JacobianType(jacobianWorkspace.workspaceForVertex(i), D, v->dimension()); + new (&_jacobianOplus[i]) JacobianType(jacobianWorkspace.workspaceForVertex(i), D < 0 ? _dimension : D, v->dimension()); } linearizeOplus(); } diff --git a/g2o/core/base_unary_edge.hpp b/g2o/core/base_unary_edge.hpp index 6d60d3348..4e125b5e3 100644 --- a/g2o/core/base_unary_edge.hpp +++ b/g2o/core/base_unary_edge.hpp @@ -83,7 +83,7 @@ void BaseUnaryEdge::constructQuadraticForm() template void BaseUnaryEdge::linearizeOplus(JacobianWorkspace& jacobianWorkspace) { - new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, VertexXiType::Dimension); + new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D < 0 ? _dimension : D, VertexXiType::Dimension); linearizeOplus(); }