diff --git a/g2o/core/optimizable_graph.cpp b/g2o/core/optimizable_graph.cpp index 4166fb836..7274de757 100644 --- a/g2o/core/optimizable_graph.cpp +++ b/g2o/core/optimizable_graph.cpp @@ -135,15 +135,17 @@ bool OptimizableGraph::Edge::resolveParameters() { for (size_t i = 0; i < _parameters.size(); i++) { int index = _parameterIds[i]; *_parameters[i] = graph()->parameter(index); + if (!*_parameters[i]) { + G2O_CRITICAL( + "parameter {} is NULL. Check setParameterId() calls for the edge.", + i); + return false; + } auto& aux = **_parameters[i]; if (typeid(aux).name() != _parameterTypes[i]) { G2O_CRITICAL("parameter type mismatch - encountered {}; should be {}", typeid(aux).name(), _parameterTypes[i]); } - if (!*_parameters[i]) { - G2O_CRITICAL("*_parameters[i] == 0"); - return false; - } } return true; } @@ -228,12 +230,12 @@ bool OptimizableGraph::addEdge(OptimizableGraph::Edge* e) { e->_internalId = _nextEdgeId++; if (e->numUndefinedVertices()) return true; if (!e->resolveParameters()) { - G2O_ERROR("{}: FATAL, cannot resolve parameters for edge {}", + G2O_ERROR("FATAL, cannot resolve parameters for edge {}", static_cast(e)); return false; } if (!e->resolveCaches()) { - G2O_ERROR("{}: FATAL, cannot resolve caches for edge {}", + G2O_ERROR("FATAL, cannot resolve caches for edge {}", static_cast(e)); return false; } @@ -263,12 +265,12 @@ bool OptimizableGraph::setEdgeVertex(HyperGraph::Edge* e, int pos, OptimizableGraph::Edge* ee = static_cast(e); #endif if (!ee->resolveParameters()) { - G2O_ERROR("{}: FATAL, cannot resolve parameters for edge {}", + G2O_ERROR("FATAL, cannot resolve parameters for edge {}", static_cast(e)); return false; } if (!ee->resolveCaches()) { - G2O_ERROR("{}: FATAL, cannot resolve caches for edge {}", + G2O_ERROR("FATAL, cannot resolve caches for edge {}", static_cast(e)); return false; } diff --git a/unit_test/general/graph_operations.cpp b/unit_test/general/graph_operations.cpp index bfec65fdd..d442691cb 100644 --- a/unit_test/general/graph_operations.cpp +++ b/unit_test/general/graph_operations.cpp @@ -42,7 +42,10 @@ #include "g2o/core/optimization_algorithm_property.h" #include "g2o/core/sparse_optimizer.h" #include "g2o/stuff/string_tools.h" -#include "g2o/types/slam2d/types_slam2d.h" +#include "g2o/types/slam2d/types_slam2d.h" // IWYU pragma: keep +#include "g2o/types/slam3d/edge_se3_pointxyz.h" +#include "g2o/types/slam3d/vertex_pointxyz.h" +#include "g2o/types/slam3d/vertex_se3.h" G2O_USE_TYPE_GROUP(slam2d); @@ -136,6 +139,27 @@ TEST(General, GraphAddEdge) { delete optimizer; } +TEST(General, GraphAddEdgeNoRequiredParam) { + g2o::SparseOptimizer* optimizer = g2o::internal::createOptimizerForTests(); + + g2o::VertexSE3* poseVertex = new g2o::VertexSE3(); + poseVertex->setId(0); + optimizer->addVertex(poseVertex); + + g2o::VertexPointXYZ* pointVertex = new g2o::VertexPointXYZ(); + pointVertex->setId(1); + optimizer->addVertex(pointVertex); + + g2o::EdgeSE3PointXYZ* edge = new g2o::EdgeSE3PointXYZ(); + edge->setVertex(0, poseVertex); + edge->setVertex(1, pointVertex); + + EXPECT_FALSE(optimizer->addEdge(edge)); + + optimizer->clear(); + delete optimizer; +} + TEST(General, GraphChangeId) { std::unique_ptr optimizer( g2o::internal::createOptimizerForTests());