diff --git a/cv_bridge/include/cv_bridge/cv_mat_sensor_msgs_image_type_adapter.hpp b/cv_bridge/include/cv_bridge/cv_mat_sensor_msgs_image_type_adapter.hpp
index c0b15a327..7902304c1 100644
--- a/cv_bridge/include/cv_bridge/cv_mat_sensor_msgs_image_type_adapter.hpp
+++ b/cv_bridge/include/cv_bridge/cv_mat_sensor_msgs_image_type_adapter.hpp
@@ -213,7 +213,7 @@ class ROSCvMatContainer
   CV_BRIDGE_PUBLIC
   bool
   is_bigendian() const;
-  
+
   /// Return the encoding override if provided.
   CV_BRIDGE_PUBLIC
   std::optional<std::string>
@@ -229,57 +229,61 @@ class ROSCvMatContainer
 
 }  // namespace cv_bridge
 
-template<>
-struct rclcpp::TypeAdapter<cv_bridge::ROSCvMatContainer, sensor_msgs::msg::Image>
+static void fill_sensor_msgs_image_from_cvmat(
+  const cv::Mat& cv_mat,
+  sensor_msgs::msg::Image& image,
+  const std_msgs::msg::Header& header,
+  const std::optional<std::string>& encoding_override = std::nullopt)
 {
-  using is_specialized = std::true_type;
-  using custom_type = cv_bridge::ROSCvMatContainer;
-  using ros_message_type = sensor_msgs::msg::Image;
+  image.height = cv_mat.rows;
+  image.width = cv_mat.cols;
 
-  static
-  void
-  convert_to_ros_message(
-    const custom_type & source,
-    ros_message_type & destination)
-  {
-    destination.height = source.cv_mat().rows;
-    destination.width = source.cv_mat().cols;
-    const auto& encoding_override = source.encoding_override();
-    if (encoding_override.has_value() && !encoding_override.value().empty())
-    {
-      destination.encoding = encoding_override.value();
-    }
-    else
-    {
-      switch (source.cv_mat().type()) {
+  if (encoding_override.has_value() && !encoding_override.value().empty()) {
+    image.encoding = encoding_override.value();
+  } else {
+    switch (cv_mat.type()) {
       case CV_8UC1:
-        destination.encoding = "mono8";
+        image.encoding = "mono8";
         break;
       case CV_8UC3:
-        destination.encoding = "bgr8";
+        image.encoding = "bgr8";
         break;
       case CV_16SC1:
-        destination.encoding = "mono16";
+        image.encoding = "mono16";
         break;
       case CV_8UC4:
-        destination.encoding = "rgba8";
+        image.encoding = "rgba8";
         break;
       default:
         throw std::runtime_error("unsupported encoding type");
-      }    
     }
-    destination.step = static_cast<sensor_msgs::msg::Image::_step_type>(source.cv_mat().step);
-    size_t size = source.cv_mat().step * source.cv_mat().rows;
-    destination.data.resize(size);
-    memcpy(&destination.data[0], source.cv_mat().data, size);
-    destination.header = source.header();
   }
 
-  static
-  void
-  convert_to_custom(
-    const ros_message_type & source,
-    custom_type & destination)
+  image.step = static_cast<sensor_msgs::msg::Image::_step_type>(cv_mat.step);
+  const size_t size = cv_mat.step * cv_mat.rows;
+  image.data.resize(size);
+  memcpy(&image.data[0], cv_mat.data, size);
+  image.header = header;
+}
+
+template<>
+struct rclcpp::TypeAdapter<cv_bridge::ROSCvMatContainer, sensor_msgs::msg::Image>
+{
+  using is_specialized = std::true_type;
+  using custom_type = cv_bridge::ROSCvMatContainer;
+  using ros_message_type = sensor_msgs::msg::Image;
+
+  static void convert_to_ros_message(
+    const custom_type& source,
+    ros_message_type& destination)
+  {
+    fill_sensor_msgs_image_from_cvmat(
+      source.cv_mat(), destination, source.header(), source.encoding_override());
+  }
+
+  static void convert_to_custom(
+    const ros_message_type& source,
+    custom_type& destination)
   {
     destination = cv_bridge::ROSCvMatContainer(source);
   }
diff --git a/cv_bridge/src/cv_mat_sensor_msgs_image_type_adapter.cpp b/cv_bridge/src/cv_mat_sensor_msgs_image_type_adapter.cpp
index 0633e3d2a..90ce92012 100644
--- a/cv_bridge/src/cv_mat_sensor_msgs_image_type_adapter.cpp
+++ b/cv_bridge/src/cv_mat_sensor_msgs_image_type_adapter.cpp
@@ -173,40 +173,10 @@ ROSCvMatContainer::get_sensor_msgs_msg_image_pointer_copy() const
   return unique_image;
 }
 
-void
-ROSCvMatContainer::get_sensor_msgs_msg_image_copy(
-  sensor_msgs::msg::Image & sensor_msgs_image) const
+void ROSCvMatContainer::get_sensor_msgs_msg_image_copy(
+  sensor_msgs::msg::Image& sensor_msgs_image) const
 {
-  sensor_msgs_image.height = frame_.rows;
-  sensor_msgs_image.width = frame_.cols;
-  if (encoding_override_.has_value() && !encoding_override_.value().empty())
-  {
-    sensor_msgs_image.encoding = encoding_override_.value();
-  }
-  else
-  {
-    switch (frame_.type()) {
-      case CV_8UC1:
-        sensor_msgs_image.encoding = "mono8";
-        break;
-      case CV_8UC3:
-        sensor_msgs_image.encoding = "bgr8";
-        break;
-      case CV_16SC1:
-        sensor_msgs_image.encoding = "mono16";
-        break;
-      case CV_8UC4:
-        sensor_msgs_image.encoding = "rgba8";
-        break;
-      default:
-        throw std::runtime_error("unsupported encoding type");
-    }
-  }
-  sensor_msgs_image.step = static_cast<sensor_msgs::msg::Image::_step_type>(frame_.step);
-  size_t size = frame_.step * frame_.rows;
-  sensor_msgs_image.data.resize(size);
-  memcpy(&sensor_msgs_image.data[0], frame_.data, size);
-  sensor_msgs_image.header = header_;
+  fill_sensor_msgs_image_from_cvmat(frame_, sensor_msgs_image, header_, encoding_override_);
 }
 
 bool