Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00050 #include <stdexcept>
00051
00052 #include <sensor_msgs/Image.h>
00053 #include <sensor_msgs/image_encodings.h>
00054 #include <boost/format.hpp>
00055
00056 #include "visp_bridge/image.h"
00057
00058 namespace visp_bridge
00059 {
00060
00061 sensor_msgs::Image toSensorMsgsImage(const vpImage<unsigned char>& src)
00062 {
00063 sensor_msgs::Image dst;
00064 dst.width = src.getWidth();
00065 dst.height = src.getHeight();
00066 dst.encoding = sensor_msgs::image_encodings::MONO8;
00067 dst.step = src.getWidth();
00068 dst.data.resize(dst.height * dst.step);
00069 memcpy(&dst.data[0], src.bitmap, dst.height * dst.step * sizeof(unsigned char));
00070
00071 return dst;
00072 }
00073
00074 vpImage<unsigned char> toVispImage(const sensor_msgs::Image& src)
00075 {
00076 using sensor_msgs::image_encodings::MONO8;
00077 using sensor_msgs::image_encodings::RGB8;
00078 using sensor_msgs::image_encodings::RGBA8;
00079 using sensor_msgs::image_encodings::BGR8;
00080 using sensor_msgs::image_encodings::BGRA8;
00081
00082
00083 vpImage<unsigned char> dst(src.height, src.width);
00084
00085 if (src.encoding == MONO8)
00086 memcpy(dst.bitmap, &(src.data[0]), dst.getHeight() * src.step * sizeof(unsigned char));
00087 else if (src.encoding == RGB8 || src.encoding == RGBA8 || src.encoding == BGR8 || src.encoding == BGRA8){
00088 unsigned nc = sensor_msgs::image_encodings::numChannels(src.encoding);
00089 unsigned cEnd = (src.encoding == RGBA8 || src.encoding == BGRA8) ? nc - 1 : nc;
00090
00091 for (unsigned i = 0; i < dst.getWidth(); ++i){
00092 for (unsigned j = 0; j < dst.getHeight(); ++j){
00093 int acc = 0;
00094 for (unsigned c = 0; c < cEnd; ++c)
00095 acc += src.data[j * src.step + i * nc + c];
00096 dst[j][i] = acc / nc;
00097 }
00098 }
00099 }
00100 return dst;
00101 }
00102
00103 vpImage<vpRGBa> toVispImageRGBa(const sensor_msgs::Image& src)
00104 {
00105 using sensor_msgs::image_encodings::MONO8;
00106 using sensor_msgs::image_encodings::RGB8;
00107 using sensor_msgs::image_encodings::RGBA8;
00108 using sensor_msgs::image_encodings::BGR8;
00109 using sensor_msgs::image_encodings::BGRA8;
00110
00111
00112 vpImage<vpRGBa> dst(src.height, src.width);
00113
00114 if (src.encoding == MONO8)
00115 for (unsigned i = 0; i < dst.getWidth(); ++i){
00116 for (unsigned j = 0; j < dst.getHeight(); ++j){
00117
00118 dst[j][i] = vpRGBa(src.data[j * src.step + i],src.data[j * src.step + i],src.data[j * src.step + i]);
00119 }
00120 }
00121 else{
00122 unsigned nc = sensor_msgs::image_encodings::numChannels(src.encoding);
00123
00124 for (unsigned i = 0; i < dst.getWidth(); ++i){
00125 for (unsigned j = 0; j < dst.getHeight(); ++j){
00126 dst[j][i] = vpRGBa(src.data[j * src.step + i * nc + 0],src.data[j * src.step + i * nc + 1],src.data[j * src.step + i * nc + 2]);
00127 }
00128 }
00129 }
00130 return dst;
00131 }
00132
00133 sensor_msgs::Image toSensorMsgsImage(const vpImage<vpRGBa>& src){
00134 sensor_msgs::Image dst;
00135 dst.width = src.getWidth();
00136 dst.height = src.getHeight();
00137 dst.encoding = sensor_msgs::image_encodings::RGB8;
00138 unsigned nc = sensor_msgs::image_encodings::numChannels(dst.encoding);
00139 dst.step = src.getWidth()*nc;
00140
00141 dst.data.resize(dst.height * dst.step);
00142 for (unsigned i = 0; i < src.getWidth(); ++i){
00143 for (unsigned j = 0; j < src.getHeight(); ++j){
00144 dst.data[j * dst.step + i * nc + 0] = src.bitmap[j * src.getWidth() + i].R;
00145 dst.data[j * dst.step + i * nc + 1] = src.bitmap[j * src.getWidth() + i].G;
00146 dst.data[j * dst.step + i * nc + 2] = src.bitmap[j * src.getWidth() + i].B;
00147
00148 }
00149 }
00150 return dst;
00151 }
00152 }