Temp fix to fill info.P matrix for canopies field rosbags (frame=1)

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)