diff --git a/src/publisher_graph.cpp b/src/publisher_graph.cpp index 2eeb8236712e7d0a283d865f555ffae23a7067ae..2c9373f876127ea0408df8127f759f209c530557 100644 --- a/src/publisher_graph.cpp +++ b/src/publisher_graph.cpp @@ -316,12 +316,12 @@ void PublisherGraph::publishFactors() // Iterate over the list of factors for (auto fac : fac_list) { - std::string fac_str = factorString(fac); - // Try to fill marker if (not fillFactorMarker(fac, factor_marker, factor_text_marker)) continue; + std::string fac_str = factorString(fac); + // markers id factor_marker.id = fac->id(); factor_text_marker.id = fac->id(); @@ -740,6 +740,10 @@ bool PublisherGraph::fillFrameMarker(FrameBaseConstPtr frm, std::string PublisherGraph::factorString(FactorBaseConstPtr fac) const { std::string factor_string; + + if (not fac or not fac->getCapture() or not fac->getCapture()->getFrame()) + return "invalid factor" + std::to_string(fac->id()); + factor_string = "F" + std::to_string(fac->getCapture()->getFrame()->id()); // FRAME diff --git a/src/publisher_pose.cpp b/src/publisher_pose.cpp index aec2e215db74543c6d05e56b6b48cad05980b54d..d1ba47180c2800e2b9c71e958b51ea59ad53677a 100644 --- a/src/publisher_pose.cpp +++ b/src/publisher_pose.cpp @@ -47,7 +47,7 @@ PublisherPose::PublisherPose(const std::string& _unique_name, max_points_ = getParamWithDefault<int>(_server, prefix_ + "/max_points", - 1e3); + 1e4); line_size_ = getParamWithDefault<double>(_server, prefix_ + "/line_size", 0.1);