From b14b95418fe54ffd1799936135d18daea73fa0cb Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Tue, 18 Jul 2023 13:15:15 +0200
Subject: [PATCH] Temp fix to fill info.P matrix for canopies field rosbags
 (frame=1)

---
 src/image_local_binarization_alg_node.cpp | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/src/image_local_binarization_alg_node.cpp b/src/image_local_binarization_alg_node.cpp
index 40f4cd6..ba2db85 100644
--- a/src/image_local_binarization_alg_node.cpp
+++ b/src/image_local_binarization_alg_node.cpp
@@ -82,6 +82,21 @@ void ImageLocalBinarizationAlgNode::image_callback(const sensor_msgs::Image::Con
       my_img = this->image_->toImageMsg();
       my_img->header.stamp = msg->header.stamp;
       this->cam_info.header.stamp = msg->header.stamp;
+
+      //BEGIN
+      //TODO: temporary for canopies field rosbags
+      if(my_img->header.frame_id=="1")
+      {
+        my_img->header.frame_id="frame1";
+        this->cam_info.header.frame_id= my_img->header.frame_id;
+        this->cam_info.P[0]=this->cam_info.K[0];
+        this->cam_info.P[5]=this->cam_info.K[4];
+        this->cam_info.P[2]=this->cam_info.K[2];
+        this->cam_info.P[6]=this->cam_info.K[5];
+        this->cam_info.P[10]=1;
+      }
+      //END
+
       this->image_out_publisher_.publish(*my_img,this->cam_info, msg->header.stamp);
     }
     catch(cv_bridge::Exception& e)
-- 
GitLab