Unifying the opengl context - one for all cameras as opposed to one f…

…or each, which seemed to have been causing issues on sw rendering
ctu-mrs · Mar 15, 2024 · 6523faf · 6523faf
1 parent 4167c00
commit 6523faf
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 9 deletions.
diff --git a/include/detect/uv_led_detect_fast_gpu.cpp b/include/detect/uv_led_detect_fast_gpu.cpp
@@ -131,9 +131,13 @@ bool uvdar::UVDARLedDetectFASTGPU::processImage(const cv::Mat i_image, std::vect
   sun_points = std::vector<cv::Point2i>();
   image_curr_     = i_image;
 
+    /* std::cerr << "[UVDARDetectorFASTGPU]: Getting image..." << std::endl; */
+
   if (!initialized_) {
     image_size = i_image.size();
+    /* std::cerr << "[UVDARDetectorFASTGPU]: First image, initializing..." << std::endl; */
     init();
+    /* return false; */
   }
 
   if (mask_id >= (int)(masks_.size())) {
@@ -165,12 +169,14 @@ bool uvdar::UVDARLedDetectFASTGPU::processImage(const cv::Mat i_image, std::vect
   compute_lib_image2d_write(&compute_prog, &texture_in, image_curr_.data);
   compute_lib_image2d_write(&compute_prog, &mask, (mask_id>=0)?masks_[mask_id].data:nullptr);
 
+  /* std::cerr << "[UVDARDetectorFASTGPU]: Dispatching..." << std::endl; */
   // dispatch compute shader
   if ( compute_lib_program_dispatch(&compute_prog, image_size.width / local_size_x, image_size.height / local_size_y, 1)){
       std::cerr << "[UVDARDetectorFASTGPU]: Failed to dispatch the shader!" << std::endl;
       return false;
   }
 
+  /* std::cerr << "[UVDARDetectorFASTGPU]: Retrieving markers..." << std::endl; */
   // retrieve detected markers
   if ( compute_lib_acbo_read_uint_val(&compute_prog, &markers_count_acbo, &markers_cnt_val)){
     std::cerr << "[UVDARDetectorFASTGPU]: Failed to extract the marker count!" << std::endl;
@@ -185,6 +191,7 @@ bool uvdar::UVDARLedDetectFASTGPU::processImage(const cv::Mat i_image, std::vect
     }
   }
 
+  /* std::cerr << "[UVDARDetectorFASTGPU]: Retrieving sun points..." << std::endl; */
   // retrieve detected sun points
   if (compute_lib_acbo_read_uint_val(&compute_prog, &sun_pts_count_acbo, &sun_points_cnt_val)){
     std::cerr << "[UVDARDetectorFASTGPU]: Failed to extract the marker count!" << std::endl;
@@ -199,6 +206,7 @@ bool uvdar::UVDARLedDetectFASTGPU::processImage(const cv::Mat i_image, std::vect
     }
   }
 
+  /* std::cerr << "[UVDARDetectorFASTGPU]: Calculating centoids..." << std::endl; */
   // find centroids of concentrated detected markers
   cpuFindMarkerCentroids(markers, markers_cnt_val, 5, detected_points);
 
@@ -216,6 +224,7 @@ bool uvdar::UVDARLedDetectFASTGPU::processImage(const cv::Mat i_image, std::vect
   /*   std::cout << "Refined: " << p << std::endl; */
   /* } */
 
+  /* std::cerr << "[UVDARDetectorFASTGPU]: Filtering markers based on sun points..." << std::endl; */
   // filter markers using detected sun points
   for (int i = 0; i < (int)(detected_points.size()); i++) { //iterate over the detected marker points
     for (int j = 0; j < (int)(sun_points.size()); j++) { //iterate over the detected sun points

diff --git a/src/detector.cpp b/src/detector.cpp
@@ -87,18 +87,18 @@ class UVDARDetector : public nodelet::Nodelet{
       detected_points_.push_back(std::vector<cv::Point>());
       sun_points_.push_back(std::vector<cv::Point>());
 
-      mutex_camera_image_.push_back(std::make_unique<std::mutex>());
+      /* mutex_camera_image_.push_back(std::make_unique<std::mutex>()); */
 
       ROS_INFO("[UVDARDetector]: Initializing FAST-based marker detection...");
-      uvdf_.push_back(std::make_unique<UVDARLedDetectFASTGPU>(
+      uvdf_ = std::make_unique<UVDARLedDetectFASTGPU>(
             _gui_,
             _debug_,
             _threshold_,
             _threshold_ / 2,
             150,
             _masks_
-            ));
-      if (!uvdf_.back()){
+            );
+      if (!uvdf_){
         ROS_ERROR("[UVDARDetector]: Failed to initialize FAST-based marker detection!");
         return;
       }
@@ -220,13 +220,15 @@ class UVDARDetector : public nodelet::Nodelet{
       return;
     }
 
+      /* ROS_INFO_STREAM("[UVDARDetector]: Locking cam image mutex " << image_index << "..."); */
     {
-      std::scoped_lock lock(*mutex_camera_image_[image_index]);
+      /* std::scoped_lock lock(*mutex_camera_image_[image_index]); */
+      std::scoped_lock lock(mutex_camera_image_);
       images_current_[image_index] = image->image;
       sun_points_[image_index].clear();
       detected_points_[image_index].clear();
 
-      if ( ! (uvdf_[image_index]->processImage(
+      if ( ! (uvdf_->processImage(
               image->image,
               detected_points_[image_index],
               sun_points_[image_index],
@@ -244,6 +246,7 @@ class UVDARDetector : public nodelet::Nodelet{
       }
       /* ROS_INFO_STREAM("There are " << sun_points_[image_index].size() << " detected potential sun points."); */
     }
+    /* ROS_INFO_STREAM("[UVDARDetector]: Unlocking cam image mutex " << image_index << "..."); */
 
     if (detected_points_[image_index].size()>MAX_POINTS_PER_IMAGE){
       ROS_WARN_STREAM("[UVDARDetector]: Over " << MAX_POINTS_PER_IMAGE << " points received. Skipping noisy image.");
@@ -317,7 +320,8 @@ class UVDARDetector : public nodelet::Nodelet{
 
     int image_index = 0;
     for ([[maybe_unused]] auto curr_size : camera_image_sizes_){
-      std::scoped_lock lock(*(mutex_camera_image_[image_index]));
+      /* std::scoped_lock lock(*(mutex_camera_image_[image_index])); */
+      std::scoped_lock lock(mutex_camera_image_);
       cv::Point start_point = cv::Point(start_widths[image_index]+image_index, 0);
       cv::Mat image_rgb;
       cv::cvtColor(images_current_[image_index], image_rgb, cv::COLOR_GRAY2BGR);
@@ -368,7 +372,8 @@ class UVDARDetector : public nodelet::Nodelet{
   bool _gui_;
   bool _publish_visualization_;
   std::unique_ptr<mrs_lib::ImagePublisher> pub_visualization_;
-  std::vector<std::unique_ptr<std::mutex>>  mutex_camera_image_;
+  /* std::vector<std::unique_ptr<std::mutex>>  mutex_camera_image_; */
+  std::mutex  mutex_camera_image_;
   ros::Timer timer_visualization_;
   ros::Timer timer_gui_visualization_;
   ros::Timer timer_publish_visualization_;
@@ -383,7 +388,8 @@ class UVDARDetector : public nodelet::Nodelet{
   std::vector<std::string> _mask_file_names_;
   std::vector<cv::Mat> _masks_;
 
-  std::vector<std::unique_ptr<UVDARLedDetectFAST>> uvdf_;
+  /* std::vector<std::unique_ptr<UVDARLedDetectFAST>> uvdf_; */
+  std::unique_ptr<UVDARLedDetectFAST> uvdf_;
   std::mutex  mutex_pub_;
   std::vector<ros::Timer> timer_process_;