diff --git a/cob_image_flip/ros/include/cob_image_flip/image_flip.h b/cob_image_flip/ros/include/cob_image_flip/image_flip.h index ae19b3a..4136a88 100644 --- a/cob_image_flip/ros/include/cob_image_flip/image_flip.h +++ b/cob_image_flip/ros/include/cob_image_flip/image_flip.h @@ -117,6 +117,27 @@ class ImageFlip void disparityConnectCB(const ros::SingleSubscriberPublisher& pub); void disparityDisconnectCB(const ros::SingleSubscriberPublisher& pub); + + + /* general: storing is carried out in row-major-style (rows change first) + * color_iamge.ptr(u) returns a pointer the specific row (u-coordinate), starting at index 0 + * [v] accesses the specific column of the pointer to the row + * For COLOR-Images (amount of channels>1): + * The order of the pixel-values in cv::Mat is as follows + * (http://blog.comart.io/assets/post-rgb/rgb-div-2e9ba3475bd9897a937811ad5a3746d84032fd2b24b7ec06356d7abf763442fb.png): + * blue(0,0), green(0, 0), red(0,0), blue(0,1), green(0,1), red(0,1), blue(0,2), green(0,2), red(0,2) + * blue(1,0), green(1, 0), red(1,0), blue(1,1), green(1,1), red(1,1), blue(1,2), green(1,2), red(1,2) + * The total number of channels has to be taken into consideration for the column-value. + * index_for_column = total_number_channels * idx_for_row + idx_for_channel + */ + template void setMatValuePtrType(cv::Mat& image, int row, int index, T value); + + template void setMatValuePtr(cv::Mat &image, int row, int index, T value); + + template T getMatValuePtrType(cv::Mat& image, int row, int index); + + double getMatValuePtr(cv::Mat& image, int row, int index); }; } + diff --git a/cob_image_flip/ros/src/image_flip.cpp b/cob_image_flip/ros/src/image_flip.cpp index a5a1461..f28a436 100644 --- a/cob_image_flip/ros/src/image_flip.cpp +++ b/cob_image_flip/ros/src/image_flip.cpp @@ -191,6 +191,56 @@ bool ImageFlip::convertImageMessageToMat(const sensor_msgs::Image::ConstPtr& ima return true; } +template void ImageFlip::setMatValuePtrType(cv::Mat& color_image, int row, int index, T value) +{ + color_image.ptr(row)[index] = value; +} + +// necessary to use typename T::type instead of just T as otherwise the signature of this function and the one above is identical -> type deduction not possible +template void ImageFlip::setMatValuePtr(cv::Mat& color_image, int row, int index, T value) +{ + switch(color_image.type() % 8) + { + // color_image.type() // 8 is related to the number of channels which doesn't matter here + // for a table about the 28 different opencv-mat-types please refer to: + // http://ninghang.blogspot.com/2012/11/list-of-mat-type-in-opencv.html + + case 0: ImageFlip::setMatValuePtrType(color_image, row, index, value); break; + case 1: ImageFlip::setMatValuePtrType(color_image, row, index, value); break; + case 2: ImageFlip::setMatValuePtrType(color_image, row, index, value); break; + case 3: ImageFlip::setMatValuePtrType(color_image, row, index, value); break; + case 4: ImageFlip::setMatValuePtrType(color_image, row, index, value); break; + case 5: ImageFlip::setMatValuePtrType(color_image, row, index, value); break; + case 6: ImageFlip::setMatValuePtrType(color_image, row, index, value); break; + + } +} + +template T ImageFlip::getMatValuePtrType(cv::Mat& color_image, int row, int index) +{ + return color_image.ptr(row)[index]; +} + +double ImageFlip::getMatValuePtr(cv::Mat& color_image, int row, int index) +{ + double value; + switch(color_image.type() % 8) + { + // color_image.type() // 8 is related to the number of channels which doesn't matter here + // for a table about the 28 different opencv-mat-types please refer to: + // http://ninghang.blogspot.com/2012/11/list-of-mat-type-in-opencv.html + + case 0: value = ImageFlip::getMatValuePtrType(color_image, row, index); break; + case 1: value = ImageFlip::getMatValuePtrType(color_image, row, index); break; + case 2: value = ImageFlip::getMatValuePtrType(color_image, row, index); break; + case 3: value = ImageFlip::getMatValuePtrType(color_image, row, index); break; + case 4: value = ImageFlip::getMatValuePtrType(color_image, row, index); break; + case 5: value = ImageFlip::getMatValuePtrType(color_image, row, index); break; + case 6: value = ImageFlip::getMatValuePtrType(color_image, row, index); break; + + } + return value; +} void ImageFlip::imageCallback(const sensor_msgs::ImageConstPtr& color_image_msg) { @@ -217,14 +267,14 @@ void ImageFlip::imageCallback(const sensor_msgs::ImageConstPtr& color_image_msg) { // rotate images by 90 degrees color_image_turned.create(color_image.cols, color_image.rows, color_image.type()); - if (color_image.type() != CV_8UC3) - { - ROS_ERROR("ImageFlip::imageCallback: Error: The image format of the color image is not CV_8UC3.\n"); - return; - } - for (int v = 0; v < color_image_turned.rows; v++) - for (int u = 0; u < color_image_turned.cols; u++) - color_image_turned.at(v,u) = color_image.at(color_image.rows-1-u,v); + int nr_channels = color_image.channels(); + for (int v = 0; v < color_image_turned.cols; v++) + for (int u = 0; u < color_image_turned.rows; u++) + for (int k = 0; k < nr_channels; k++) + { + + ImageFlip::setMatValuePtr(color_image_turned, u, (color_image_turned.cols-1-v)*nr_channels+k, ImageFlip::getMatValuePtr(color_image,v,nr_channels*u+k)); + } rot_mat.at(0,1) = -1.; rot_mat.at(0,2) = color_image.rows; rot_mat.at(1,0) = 1.; @@ -233,14 +283,12 @@ void ImageFlip::imageCallback(const sensor_msgs::ImageConstPtr& color_image_msg) { // rotate images by 270 degrees color_image_turned.create(color_image.cols, color_image.rows, color_image.type()); - if (color_image.type() != CV_8UC3) - { - std::cout << "ImageFlip::imageCallback: Error: The image format of the color image is not CV_8UC3.\n"; - return; - } - for (int v = 0; v < color_image_turned.rows; v++) - for (int u = 0; u < color_image_turned.cols; u++) - color_image_turned.at(v,u) = color_image.at(u,color_image.cols-1-v); + int nr_channels = color_image.channels(); + for (int v = 0; v < color_image_turned.cols; v++) + for (int u = 0; u < color_image_turned.rows; u++) + for (int k = 0; k < nr_channels; k++) + ImageFlip::setMatValuePtr(color_image_turned, (color_image_turned.rows -1 -u), v*nr_channels + k, ImageFlip::getMatValuePtr(color_image, v, nr_channels * u + k)); + rot_mat.at(0,1) = 1.; rot_mat.at(1,0) = -1.; rot_mat.at(1,2) = color_image.cols; @@ -249,26 +297,12 @@ void ImageFlip::imageCallback(const sensor_msgs::ImageConstPtr& color_image_msg) { // rotate images by 180 degrees color_image_turned.create(color_image.rows, color_image.cols, color_image.type()); - if (color_image.type() != CV_8UC3) - { - std::cout << "ImageFlip::imageCallback: Error: The image format of the color image is not CV_8UC3.\n"; - return; - } - for (int v = 0; v < color_image.rows; v++) - { - uchar* src = color_image.ptr(v); - uchar* dst = color_image_turned.ptr(color_image.rows - v - 1) + 3 * (color_image.cols - 1); - for (int u = 0; u < color_image.cols; u++) - { - for (int k = 0; k < 3; k++) - { - *dst = *src; - src++; - dst++; - } - dst -= 6; - } - } + int nr_channels = color_image.channels(); + for (int v = 0; v < color_image_turned.cols; v++) + for (int u = 0; u < color_image_turned.rows; u++) + for (int k = 0; k < nr_channels; k++) + ImageFlip::setMatValuePtr(color_image_turned, (color_image.rows -1 - v), nr_channels*(color_image.cols - 1 -u) + k, ImageFlip::getMatValuePtr(color_image, v, nr_channels * u + k)); + rot_mat.at(0,0) = -1.; rot_mat.at(0,2) = color_image.cols; rot_mat.at(1,1) = -1.; @@ -282,11 +316,6 @@ void ImageFlip::imageCallback(const sensor_msgs::ImageConstPtr& color_image_msg) color_image_turned.create(color_image.rows, color_image.cols, color_image.type()); // automatically decide for landscape or portrait orientation of resulting image else color_image_turned.create(color_image.cols, color_image.rows, color_image.type()); - if (color_image.type() != CV_8UC3) - { - ROS_ERROR("ImageFlip::imageCallback: Error: The image format of the color image is not CV_8UC3.\n"); - return; - } cv::Point center = cv::Point(color_image.cols/2, color_image.rows/2); rot_mat = cv::getRotationMatrix2D(center, -rotation_angle, 1.0); @@ -607,14 +636,12 @@ void ImageFlip::disparityCallback(const stereo_msgs::DisparityImage::ConstPtr& d { // rotate images by 90 degrees disparity_image_turned.create(disparity_image.cols, disparity_image.rows, disparity_image.type()); - if (disparity_image.type() != CV_32FC1) - { - ROS_ERROR("ImageFlip::imageCallback: Error: The image format of the disparity image is not CV_32FC1.\n"); - return; - } + int nr_channels = disparity_image.channels(); for (int v = 0; v < disparity_image_turned.rows; v++) for (int u = 0; u < disparity_image_turned.cols; u++) - disparity_image_turned.at(v,u) = disparity_image.at(disparity_image.rows-1-u,v); + for (int k = 0; k < nr_channels; k++) + ImageFlip::setMatValuePtr(disparity_image_turned,u,(disparity_image.rows-1-v)*nr_channels+k, ImageFlip::getMatValuePtr(disparity_image,v,nr_channels*u+k)); + rot_mat.at(0,1) = -1.; rot_mat.at(0,2) = disparity_image.rows; rot_mat.at(1,0) = 1.; @@ -623,14 +650,12 @@ void ImageFlip::disparityCallback(const stereo_msgs::DisparityImage::ConstPtr& d { // rotate images by 270 degrees disparity_image_turned.create(disparity_image.cols, disparity_image.rows, disparity_image.type()); - if (disparity_image.type() != CV_32FC1) - { - std::cout << "ImageFlip::imageCallback: Error: The image format of the color image is not CV_32FC1.\n"; - return; - } + int nr_channels = disparity_image.channels(); for (int v = 0; v < disparity_image_turned.rows; v++) for (int u = 0; u < disparity_image_turned.cols; u++) - disparity_image_turned.at(v,u) = disparity_image.at(u,disparity_image.cols-1-v); + for (int k = 0; k < nr_channels; k++) + ImageFlip::setMatValuePtr(disparity_image_turned, (disparity_image.cols-1-u),v*nr_channels+k, ImageFlip::getMatValuePtr(disparity_image,v,nr_channels*u+k)); + rot_mat.at(0,1) = 1.; rot_mat.at(1,0) = -1.; rot_mat.at(1,2) = disparity_image.cols; @@ -639,22 +664,12 @@ void ImageFlip::disparityCallback(const stereo_msgs::DisparityImage::ConstPtr& d { // rotate images by 180 degrees disparity_image_turned.create(disparity_image.rows, disparity_image.cols, disparity_image.type()); - if (disparity_image.type() != CV_32FC1) - { - std::cout << "ImageFlip::imageCallback: Error: The image format of the color image is not CV_32FC1.\n"; - return; - } + int nr_channels = disparity_image.channels(); for (int v = 0; v < disparity_image.rows; v++) - { - float* src = (float*)disparity_image.ptr(v); - float* dst = (float*)disparity_image_turned.ptr(disparity_image.rows - v - 1) + (disparity_image.cols - 1); for (int u = 0; u < disparity_image.cols; u++) - { - *dst = *src; - src++; - dst--; - } - } + for (int k = 0; k < nr_channels; k++) + ImageFlip::setMatValuePtr(disparity_image_turned, (disparity_image.rows-1-v), nr_channels*(disparity_image.cols-1-u)+k, ImageFlip::getMatValuePtr(disparity_image,v,nr_channels*u+k)); + rot_mat.at(0,0) = -1.; rot_mat.at(0,2) = disparity_image.cols; rot_mat.at(1,1) = -1.; @@ -668,11 +683,6 @@ void ImageFlip::disparityCallback(const stereo_msgs::DisparityImage::ConstPtr& d disparity_image_turned.create(disparity_image.rows, disparity_image.cols, disparity_image.type()); // automatically decide for landscape or portrait orientation of resulting image else disparity_image_turned.create(disparity_image.cols, disparity_image.rows, disparity_image.type()); - if (disparity_image.type() != CV_32FC1) - { - ROS_ERROR("ImageFlip::imageCallback: Error: The image format of the color image is not CV_32FC1.\n"); - return; - } cv::Point center = cv::Point(disparity_image.cols/2, disparity_image.rows/2); rot_mat = cv::getRotationMatrix2D(center, -rotation_angle, 1.0); @@ -734,3 +744,4 @@ void ImageFlip::disparityDisconnectCB(const ros::SingleSubscriberPublisher& pub) } +