From 136099cd6c54d46fcd231105013e983fe604880b Mon Sep 17 00:00:00 2001
From: ymd-stella <7959916+ymd-stella@users.noreply.github.com>
Date: Sun, 25 Feb 2024 09:54:32 +0900
Subject: [PATCH] Add parameter encoding (default is empty) (#173)

---
 src/stella_vslam_ros.cc | 15 +++++++++------
 src/stella_vslam_ros.h  |  2 ++
 2 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/src/stella_vslam_ros.cc b/src/stella_vslam_ros.cc
index 6ffecbb..10c9565 100644
--- a/src/stella_vslam_ros.cc
+++ b/src/stella_vslam_ros.cc
@@ -142,6 +142,9 @@ void system::setParams() {
 
     transform_tolerance_ = 0.5;
     transform_tolerance_ = node_->declare_parameter("transform_tolerance", transform_tolerance_);
+
+    encoding_ = "";
+    encoding_ = node_->declare_parameter("encoding", encoding_);
 }
 
 void system::init_pose_callback(
@@ -227,7 +230,7 @@ void mono::callback(sensor_msgs::msg::Image::UniquePtr msg_unique_ptr) {
     const double timestamp = rclcpp::Time(msg->header.stamp).seconds();
 
     // input the current frame and estimate the camera pose
-    auto cam_pose_wc = slam_->feed_monocular_frame(cv_bridge::toCvShare(msg)->image, timestamp, mask_);
+    auto cam_pose_wc = slam_->feed_monocular_frame(cv_bridge::toCvShare(msg, encoding_)->image, timestamp, mask_);
 
     const rclcpp::Time tp_2 = node_->now();
     const double track_time = (tp_2 - tp_1).seconds();
@@ -251,7 +254,7 @@ void mono::callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) {
     const double timestamp = rclcpp::Time(msg->header.stamp).seconds();
 
     // input the current frame and estimate the camera pose
-    auto cam_pose_wc = slam_->feed_monocular_frame(cv_bridge::toCvShare(msg)->image, timestamp, mask_);
+    auto cam_pose_wc = slam_->feed_monocular_frame(cv_bridge::toCvShare(msg, encoding_)->image, timestamp, mask_);
 
     const rclcpp::Time tp_2 = node_->now();
     const double track_time = (tp_2 - tp_1).seconds();
@@ -291,8 +294,8 @@ void stereo::callback(const sensor_msgs::msg::Image::ConstSharedPtr& left, const
     if (camera_optical_frame_.empty()) {
         camera_optical_frame_ = left->header.frame_id;
     }
-    auto leftcv = cv_bridge::toCvShare(left)->image;
-    auto rightcv = cv_bridge::toCvShare(right)->image;
+    auto leftcv = cv_bridge::toCvShare(left, encoding_)->image;
+    auto rightcv = cv_bridge::toCvShare(right, encoding_)->image;
     if (leftcv.empty() || rightcv.empty()) {
         return;
     }
@@ -343,8 +346,8 @@ void rgbd::callback(const sensor_msgs::msg::Image::ConstSharedPtr& color, const
     if (camera_optical_frame_.empty()) {
         camera_optical_frame_ = color->header.frame_id;
     }
-    auto colorcv = cv_bridge::toCvShare(color)->image;
-    auto depthcv = cv_bridge::toCvShare(depth)->image;
+    auto colorcv = cv_bridge::toCvShare(color, encoding_)->image;
+    auto depthcv = cv_bridge::toCvShare(depth, encoding_)->image;
     if (colorcv.empty() || depthcv.empty()) {
         return;
     }
diff --git a/src/stella_vslam_ros.h b/src/stella_vslam_ros.h
index 998fd2d..35b7c16 100644
--- a/src/stella_vslam_ros.h
+++ b/src/stella_vslam_ros.h
@@ -65,6 +65,8 @@ class system {
     // If true, odom_frame is fixed on the xy-plane of map_frame. This is useful when working with 2D navigation modules.
     bool odom2d_;
 
+    std::string encoding_;
+
 private:
     void init_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);