ros_image_texture.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <map>
31 #include <sstream>
32 #include <algorithm>
33 #include <iostream>
34 
35 #include <boost/algorithm/string/erase.hpp>
36 #include <boost/foreach.hpp>
37 
38 #include <OgreTextureManager.h>
39 
41 
43 
44 namespace rviz
45 {
46 ROSImageTexture::ROSImageTexture() : new_image_(false), median_frames_(5)
47 {
48  empty_image_.load("no_image.png", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
49  width_ = empty_image_.getWidth();
50  height_ = empty_image_.getHeight();
51 
52  static uint32_t count = 0;
53  std::stringstream ss;
54  ss << "ROSImageTexture" << count++;
55  texture_ = Ogre::TextureManager::getSingleton().loadImage(
56  ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, empty_image_, Ogre::TEX_TYPE_2D,
57  0);
58 
60 }
61 
63 {
64  current_image_.reset();
65 }
66 
68 {
69  boost::mutex::scoped_lock lock(mutex_);
70 
71  texture_->unload();
72  texture_->loadImage(empty_image_);
73  width_ = empty_image_.getWidth();
74  height_ = empty_image_.getHeight();
75 
76  new_image_ = false;
77  current_image_.reset();
78 }
79 
80 const sensor_msgs::Image::ConstPtr& ROSImageTexture::getImage()
81 {
82  boost::mutex::scoped_lock lock(mutex_);
83 
84  return current_image_;
85 }
86 
87 void ROSImageTexture::setMedianFrames(unsigned median_frames)
88 {
89  median_frames_ = median_frames;
90 }
91 
92 double ROSImageTexture::updateMedian(std::deque<double>& buffer, double value)
93 {
94  // update buffer
95  while (buffer.size() > median_frames_ - 1)
96  {
97  buffer.pop_back();
98  }
99  buffer.push_front(value);
100  // get median
101  std::deque<double> buffer2 = buffer;
102  std::nth_element(buffer2.begin(), buffer2.begin() + buffer2.size() / 2, buffer2.end());
103  return *(buffer2.begin() + buffer2.size() / 2);
104 }
105 
106 void ROSImageTexture::setNormalizeFloatImage(bool normalize, double min, double max)
107 {
109  min_ = min;
110  max_ = max;
111 }
112 
113 
114 template <typename T>
115 void ROSImageTexture::normalize(T* image_data, size_t image_data_size, std::vector<uint8_t>& buffer)
116 {
117  // Prepare output buffer
118  buffer.resize(image_data_size, 0);
119 
120  T minValue;
121  T maxValue;
122 
123  if (normalize_)
124  {
125  T* input_ptr = image_data;
126  // Find min. and max. pixel value
127  minValue = std::numeric_limits<T>::max();
128  maxValue = std::numeric_limits<T>::min();
129  for (unsigned i = 0; i < image_data_size; ++i)
130  {
131  if (*input_ptr == std::numeric_limits<double>::infinity() ||
132  *input_ptr == -std::numeric_limits<double>::infinity() ||
133  *input_ptr == std::numeric_limits<double>::quiet_NaN())
134  {
135  input_ptr++;
136  continue;
137  }
138 
139  minValue = std::min(minValue, *input_ptr);
140  maxValue = std::max(maxValue, *input_ptr);
141  input_ptr++;
142  }
143 
144  if (median_frames_ > 1)
145  {
146  minValue = updateMedian(min_buffer_, minValue);
147  maxValue = updateMedian(max_buffer_, maxValue);
148  }
149  }
150  else
151  {
152  // set fixed min/max
153  minValue = min_;
154  maxValue = max_;
155  }
156 
157  // Rescale floating point image and convert it to 8-bit
158  double range = maxValue - minValue;
159  if (range > 0.0)
160  {
161  T* input_ptr = image_data;
162 
163  // Pointer to output buffer
164  uint8_t* output_ptr = &buffer[0];
165 
166  // Rescale and quantize
167  for (size_t i = 0; i < image_data_size; ++i, ++output_ptr, ++input_ptr)
168  {
169  double val = (double(*input_ptr - minValue) / range);
170  if (val < 0)
171  val = 0;
172  if (val > 1)
173  val = 1;
174  *output_ptr = val * 255u;
175  }
176  }
177 }
178 
180 {
181  sensor_msgs::Image::ConstPtr image;
182  bool new_image = false;
183  {
184  boost::mutex::scoped_lock lock(mutex_);
185 
186  image = current_image_;
187  new_image = new_image_;
188  }
189 
190  if (!image || !new_image)
191  {
192  return false;
193  }
194 
195  new_image_ = false;
196 
197  if (image->data.empty())
198  {
199  return false;
200  }
201 
202  Ogre::PixelFormat format = Ogre::PF_R8G8B8;
203  Ogre::Image ogre_image;
204  std::vector<uint8_t> buffer;
205 
206  uint8_t* imageDataPtr = (uint8_t*)&image->data[0];
207  size_t imageDataSize = image->data.size();
208 
209  if (image->encoding == sensor_msgs::image_encodings::RGB8)
210  {
211  format = Ogre::PF_BYTE_RGB;
212  }
213  else if (image->encoding == sensor_msgs::image_encodings::RGBA8)
214  {
215  format = Ogre::PF_BYTE_RGBA;
216  }
217  else if (image->encoding == sensor_msgs::image_encodings::TYPE_8UC4 ||
218  image->encoding == sensor_msgs::image_encodings::TYPE_8SC4 ||
219  image->encoding == sensor_msgs::image_encodings::BGRA8)
220  {
221  format = Ogre::PF_BYTE_BGRA;
222  }
223  else if (image->encoding == sensor_msgs::image_encodings::TYPE_8UC3 ||
224  image->encoding == sensor_msgs::image_encodings::TYPE_8SC3 ||
225  image->encoding == sensor_msgs::image_encodings::BGR8)
226  {
227  format = Ogre::PF_BYTE_BGR;
228  }
229  else if (image->encoding == sensor_msgs::image_encodings::TYPE_8UC1 ||
230  image->encoding == sensor_msgs::image_encodings::TYPE_8SC1 ||
231  image->encoding == sensor_msgs::image_encodings::MONO8)
232  {
233  format = Ogre::PF_BYTE_L;
234  }
235  else if (image->encoding == sensor_msgs::image_encodings::TYPE_16UC1 ||
236  image->encoding == sensor_msgs::image_encodings::TYPE_16SC1 ||
237  image->encoding == sensor_msgs::image_encodings::MONO16)
238  {
239  imageDataSize /= sizeof(uint16_t);
240  normalize<uint16_t>((uint16_t*)&image->data[0], imageDataSize, buffer);
241  format = Ogre::PF_BYTE_L;
242  imageDataPtr = &buffer[0];
243  }
244  else if (image->encoding.find("bayer") == 0)
245  {
246  format = Ogre::PF_BYTE_L;
247  }
248  else if (image->encoding == sensor_msgs::image_encodings::TYPE_32FC1)
249  {
250  imageDataSize /= sizeof(float);
251  normalize<float>((float*)&image->data[0], imageDataSize, buffer);
252  format = Ogre::PF_BYTE_L;
253  imageDataPtr = &buffer[0];
254  }
255  else
256  {
257  throw UnsupportedImageEncoding(image->encoding);
258  }
259 
260  width_ = image->width;
261  height_ = image->height;
262 
263  // TODO: Support different steps/strides
264 
265  Ogre::DataStreamPtr pixel_stream;
266  pixel_stream.bind(new Ogre::MemoryDataStream(imageDataPtr, imageDataSize));
267 
268  try
269  {
270  ogre_image.loadRawData(pixel_stream, width_, height_, 1, format, 1, 0);
271  }
272  catch (Ogre::Exception& e)
273  {
274  // TODO: signal error better
275  ROS_ERROR("Error loading image: %s", e.what());
276  return false;
277  }
278 
279  texture_->unload();
280  texture_->loadImage(ogre_image);
281 
282  return true;
283 }
284 
285 void ROSImageTexture::addMessage(const sensor_msgs::Image::ConstPtr& msg)
286 {
287  boost::mutex::scoped_lock lock(mutex_);
288  current_image_ = msg;
289  new_image_ = true;
290 }
291 
292 } // end of namespace rviz
const std::string TYPE_8SC4
const std::string TYPE_8UC4
Ogre::TexturePtr texture_
double updateMedian(std::deque< double > &buffer, double new_value)
const std::string TYPE_8UC1
std::deque< double > max_buffer_
sensor_msgs::Image::ConstPtr current_image_
void setNormalizeFloatImage(bool normalize, double min=0.0, double max=1.0)
void setMedianFrames(unsigned median_frames)
void normalize(T *image_data, size_t image_data_size, std::vector< uint8_t > &buffer)
const std::string TYPE_8SC3
void addMessage(const sensor_msgs::Image::ConstPtr &image)
const std::string TYPE_32FC1
const std::string TYPE_16UC1
const std::string TYPE_8SC1
const std::string MONO16
std::deque< double > min_buffer_
const std::string TYPE_8UC3
const sensor_msgs::Image::ConstPtr & getImage()
#define ROS_ERROR(...)
const std::string TYPE_16SC1


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat May 27 2023 02:06:25