fixing compressed color format to comply with opencv api

ros-perception · Dec 7, 2012 · 8a029c4 · 8a029c4
1 parent 39710e8
commit 8a029c4
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 12 deletions.
diff --git a/compressed_image_transport/src/compressed_publisher.cpp b/compressed_image_transport/src/compressed_publisher.cpp
@@ -68,7 +68,7 @@ void CompressedPublisher::publish(const sensor_msgs::Image& message, const Publi
       params[1] = config_.jpeg_quality;
 
       // Update ros message format header
-      compressed.format += "; jpeg compressed";
+      compressed.format += "; jpeg compressed ";
 
       // Check input format
       if ((bitDepth == 8) && // JPEG only works on 8bit images
@@ -80,7 +80,8 @@ void CompressedPublisher::publish(const sensor_msgs::Image& message, const Publi
         if (enc::isColor(message.encoding))
         {
           // convert color images to RGB domain
-          targetFormat << "rgb" << bitDepth;
+          targetFormat << "bgr" << bitDepth;
+          compressed.format += targetFormat.str();
         }
 
         // OpenCV-ros bridge
@@ -126,7 +127,7 @@ void CompressedPublisher::publish(const sensor_msgs::Image& message, const Publi
       params[1] = config_.png_level;
 
       // Update ros message format header
-      compressed.format += "; png compressed";
+      compressed.format += "; png compressed ";
 
       // Check input format
       if (((bitDepth == 16) || (bitDepth == 8)) && ((numChannels == 1) || (numChannels == 3)))
@@ -137,7 +138,8 @@ void CompressedPublisher::publish(const sensor_msgs::Image& message, const Publi
         if (enc::isColor(message.encoding))
         {
           // convert color images to RGB domain
-          targetFormat << "rgb" << bitDepth;
+          targetFormat << "bgr" << bitDepth;
+          compressed.format += targetFormat.str();
         }
 
         // OpenCV-ros bridge

diff --git a/compressed_image_transport/src/compressed_subscriber.cpp b/compressed_image_transport/src/compressed_subscriber.cpp
@@ -51,20 +51,40 @@ void CompressedSubscriber::internalCallback(const sensor_msgs::CompressedImageCo
       }
     } else
     {
-      string image_encoding = message->format.substr(0, message->format.find(';'));
+      string image_encoding = message->format.substr(0, split_pos);
+
       cv_ptr->encoding = image_encoding;
 
       if ( enc::isColor(image_encoding))
       {
-        // Revert color transformation
-        if ((image_encoding == enc::BGR8) || (image_encoding == enc::BGR16))
-          cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGR);
+        string compressed_encoding = message->format.substr(split_pos);
+        bool compressed_bgr_image = (compressed_encoding.find("compressed bgr")!=string::npos);
 
-        if ((image_encoding == enc::BGRA8) || (image_encoding == enc::BGRA16))
-          cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGRA);
+        // Revert color transformation
+        if (compressed_bgr_image)
+        {
+          // if necessary convert colors from bgr to rgb
+          if ((image_encoding == enc::RGB8) || (image_encoding == enc::RGB16))
+            cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGB);
+
+          if ((image_encoding == enc::RGBA8) || (image_encoding == enc::RGBA16))
+            cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2RGBA);
+
+          if ((image_encoding == enc::BGRA8) || (image_encoding == enc::BGRA16))
+            cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_BGR2BGRA);
+        } else
+        {
+          // if necessary convert colors from rgb to bgr
+          if ((image_encoding == enc::BGR8) || (image_encoding == enc::BGR16))
+            cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGR);
+
+          if ((image_encoding == enc::BGRA8) || (image_encoding == enc::BGRA16))
+            cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2BGRA);
+
+          if ((image_encoding == enc::RGBA8) || (image_encoding == enc::RGBA16))
+            cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2RGBA);
+        }
 
-        if ((image_encoding == enc::RGBA8) || (image_encoding == enc::RGBA16))
-          cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2RGBA);
       }
     }
   }