From 9042c9ef38eb3656a89f0cdf2b985adf1458f3a1 Mon Sep 17 00:00:00 2001
From: Rainer Kuemmerle <rainer.kuemmerle@gmail.com>
Date: Sat, 14 Dec 2024 11:57:29 +0100
Subject: [PATCH] Fix adding edge with broken parameters

---
 g2o/core/optimizable_graph.cpp | 18 ++++++++++--------
 1 file changed, 10 insertions(+), 8 deletions(-)

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<void*>(e));
     return false;
   }
   if (!e->resolveCaches()) {
-    G2O_ERROR("{}: FATAL, cannot resolve caches for edge {}",
+    G2O_ERROR("FATAL, cannot resolve caches for edge {}",
               static_cast<void*>(e));
     return false;
   }
@@ -263,12 +265,12 @@ bool OptimizableGraph::setEdgeVertex(HyperGraph::Edge* e, int pos,
     OptimizableGraph::Edge* ee = static_cast<OptimizableGraph::Edge*>(e);
 #endif
     if (!ee->resolveParameters()) {
-      G2O_ERROR("{}: FATAL, cannot resolve parameters for edge {}",
+      G2O_ERROR("FATAL, cannot resolve parameters for edge {}",
                 static_cast<void*>(e));
       return false;
     }
     if (!ee->resolveCaches()) {
-      G2O_ERROR("{}: FATAL, cannot resolve caches for edge {}",
+      G2O_ERROR("FATAL, cannot resolve caches for edge {}",
                 static_cast<void*>(e));
       return false;
     }