From f81157cfc633349ef52154d7a1c96f99320dfa98 Mon Sep 17 00:00:00 2001 From: hollydinkel Date: Tue, 23 Jan 2024 08:30:44 -0600 Subject: [PATCH] [FIX] re-add code snippet for publishing self-occluded nodes --- trackdlo/src/trackdlo_node.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/trackdlo/src/trackdlo_node.cpp b/trackdlo/src/trackdlo_node.cpp index cb86eb9..4142337 100644 --- a/trackdlo/src/trackdlo_node.cpp +++ b/trackdlo/src/trackdlo_node.cpp @@ -322,11 +322,14 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons } // add edges for checking overlap with upcoming nodes - double x1 = col_1; - double y1 = row_1; - double x2 = col_2; - double y2 = row_2; - cv::line(projected_edges, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(255, 255, 255), dlo_pixel_width); + cv::line(projected_edges, cv::Point(col_1, row_1), cv::Point(col_2, row_2), cv::Scalar(255, 255, 255), dlo_pixel_width); + } + + // obtain self-occluded nodes + for (int i = 0; i < Y.rows(); i ++) { + if (std::find(not_self_occluded_nodes.begin(), not_self_occluded_nodes.end(), i) == not_self_occluded_nodes.end()) { + self_occluded_nodes.push_back(i); + } } // sort visible nodes to preserve the original connectivity