image.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * $Id: image.cpp 3496 2011-11-22 15:14:32Z fnovotny $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  * Contact visp@irisa.fr if any conditions of this licensing are
34  * not clear to you.
35  *
36  * Description:
37  *
38  *
39  * Authors:
40  * Filip Novotny
41  *
42  *
43  *****************************************************************************/
44 
50 #include <stdexcept>
51 
52 #include <sensor_msgs/Image.h>
54 #include <boost/format.hpp>
55 
56 #include "visp_bridge/image.h"
57 
58 namespace visp_bridge
59 {
60 
61 sensor_msgs::Image toSensorMsgsImage(const vpImage<unsigned char>& src)
62 {
63  sensor_msgs::Image dst;
64  dst.width = src.getWidth();
65  dst.height = src.getHeight();
67  dst.step = src.getWidth();
68  dst.data.resize(dst.height * dst.step);
69  memcpy(&dst.data[0], src.bitmap, dst.height * dst.step * sizeof(unsigned char));
70 
71  return dst;
72 }
73 
74 vpImage<unsigned char> toVispImage(const sensor_msgs::Image& src)
75 {
81 
82 
83  vpImage<unsigned char> dst(src.height, src.width);
84 
85  if (src.encoding == MONO8)
86  memcpy(dst.bitmap, &(src.data[0]), dst.getHeight() * src.step * sizeof(unsigned char));
87  else if (src.encoding == RGB8 || src.encoding == RGBA8 || src.encoding == BGR8 || src.encoding == BGRA8){
88  unsigned nc = sensor_msgs::image_encodings::numChannels(src.encoding);
89  unsigned cEnd = (src.encoding == RGBA8 || src.encoding == BGRA8) ? nc - 1 : nc;
90 
91  for (unsigned i = 0; i < dst.getWidth(); ++i){
92  for (unsigned j = 0; j < dst.getHeight(); ++j){
93  int acc = 0;
94  for (unsigned c = 0; c < cEnd; ++c)
95  acc += src.data[j * src.step + i * nc + c];
96  dst[j][i] = acc / nc;
97  }
98  }
99  }
100  return dst;
101 }
102 
103 vpImage<vpRGBa> toVispImageRGBa(const sensor_msgs::Image& src)
104 {
110 
111 
112  vpImage<vpRGBa> dst(src.height, src.width);
113 
114  if (src.encoding == MONO8)
115  for (unsigned i = 0; i < dst.getWidth(); ++i){
116  for (unsigned j = 0; j < dst.getHeight(); ++j){
117 
118  dst[j][i] = vpRGBa(src.data[j * src.step + i],src.data[j * src.step + i],src.data[j * src.step + i]);
119  }
120  }
121  else{
122  unsigned nc = sensor_msgs::image_encodings::numChannels(src.encoding);
123 
124  for (unsigned i = 0; i < dst.getWidth(); ++i){
125  for (unsigned j = 0; j < dst.getHeight(); ++j){
126  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]);
127  }
128  }
129  }
130  return dst;
131 }
132 
133 sensor_msgs::Image toSensorMsgsImage(const vpImage<vpRGBa>& src){
134  sensor_msgs::Image dst;
135  dst.width = src.getWidth();
136  dst.height = src.getHeight();
137  dst.encoding = sensor_msgs::image_encodings::RGB8;
138  unsigned nc = sensor_msgs::image_encodings::numChannels(dst.encoding);
139  dst.step = src.getWidth()*nc;
140 
141  dst.data.resize(dst.height * dst.step);
142  for (unsigned i = 0; i < src.getWidth(); ++i){
143  for (unsigned j = 0; j < src.getHeight(); ++j){
144  dst.data[j * dst.step + i * nc + 0] = src.bitmap[j * src.getWidth() + i].R;
145  dst.data[j * dst.step + i * nc + 1] = src.bitmap[j * src.getWidth() + i].G;
146  dst.data[j * dst.step + i * nc + 2] = src.bitmap[j * src.getWidth() + i].B;
147  //dst.data[j * dst.step + i * nc + 3] = src.bitmap[j * dst.step + i].A;
148  }
149  }
150  return dst;
151 }
152 }
vpImage< unsigned char > toVispImage(const sensor_msgs::Image &src)
Converts a sensor_msgs::Image to a ViSP image (vpImage). Only works for greyscale images...
Definition: image.cpp:74
Defines conversions between ViSP and ROS image types.
sensor_msgs::Image toSensorMsgsImage(const vpImage< unsigned char > &src)
Converts a ViSP image (vpImage) to a sensor_msgs::Image. Only works for greyscale images...
Definition: image.cpp:61
static int numChannels(const std::string &encoding)
vpImage< vpRGBa > toVispImageRGBa(const sensor_msgs::Image &src)
Definition: image.cpp:103


visp_bridge
Author(s): Filip Novotny
autogenerated on Wed Jul 3 2019 19:48:02