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 
48 : new_image_(false)
49 , width_(0)
50 , height_(0)
51 , median_frames_(5)
52 {
53  empty_image_.load("no_image.png", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
54 
55  static uint32_t count = 0;
56  std::stringstream ss;
57  ss << "ROSImageTexture" << count++;
58  texture_ = Ogre::TextureManager::getSingleton().loadImage(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, empty_image_, Ogre::TEX_TYPE_2D, 0);
59 
61 }
62 
64 {
65  current_image_.reset();
66 }
67 
69 {
70  boost::mutex::scoped_lock lock(mutex_);
71 
72  texture_->unload();
73  texture_->loadImage(empty_image_);
74 
75  new_image_ = false;
76  current_image_.reset();
77 }
78 
79 const sensor_msgs::Image::ConstPtr& ROSImageTexture::getImage()
80 {
81  boost::mutex::scoped_lock lock(mutex_);
82 
83  return current_image_;
84 }
85 
86 void ROSImageTexture::setMedianFrames( unsigned median_frames )
87 {
88  median_frames_ = median_frames;
89 }
90 
91 double ROSImageTexture::updateMedian( std::deque<double>& buffer, double value )
92 {
93  //update buffer
94  while(buffer.size() > median_frames_-1)
95  {
96  buffer.pop_back();
97  }
98  buffer.push_front(value);
99  // get median
100  std::deque<double> buffer2 = buffer;
101  std::nth_element( buffer2.begin(), buffer2.begin()+buffer2.size()/2, buffer2.end() );
102  return *( buffer2.begin()+buffer2.size()/2 );
103 }
104 
105 void ROSImageTexture::setNormalizeFloatImage( bool normalize, double min, double max )
106 {
108  min_ = min;
109  max_ = max;
110 }
111 
112 
113 template<typename T>
114 void ROSImageTexture::normalize( T* image_data, size_t image_data_size, std::vector<uint8_t> &buffer )
115 {
116  // Prepare output buffer
117  buffer.resize(image_data_size, 0);
118 
119  T minValue;
120  T maxValue;
121 
122  if ( normalize_ )
123  {
124  T* input_ptr = image_data;
125  // Find min. and max. pixel value
126  minValue = std::numeric_limits<T>::max();
127  maxValue = std::numeric_limits<T>::min();
128  for( unsigned i = 0; i < image_data_size; ++i )
129  {
130  minValue = std::min( minValue, *input_ptr );
131  maxValue = std::max( maxValue, *input_ptr );
132  input_ptr++;
133  }
134 
135  if ( median_frames_ > 1 )
136  {
137  minValue = updateMedian( min_buffer_, minValue );
138  maxValue = updateMedian( max_buffer_, maxValue );
139  }
140  }
141  else
142  {
143  // set fixed min/max
144  minValue = min_;
145  maxValue = max_;
146  }
147 
148  // Rescale floating point image and convert it to 8-bit
149  double range = maxValue - minValue;
150  if( range > 0.0 )
151  {
152  T* input_ptr = image_data;
153 
154  // Pointer to output buffer
155  uint8_t* output_ptr = &buffer[0];
156 
157  // Rescale and quantize
158  for( size_t i = 0; i < image_data_size; ++i, ++output_ptr, ++input_ptr )
159  {
160  double val = (double(*input_ptr - minValue) / range);
161  if ( val < 0 ) val = 0;
162  if ( val > 1 ) val = 1;
163  *output_ptr = val * 255u;
164  }
165  }
166 }
167 
169 {
170  sensor_msgs::Image::ConstPtr image;
171  bool new_image = false;
172  {
173  boost::mutex::scoped_lock lock(mutex_);
174 
175  image = current_image_;
176  new_image = new_image_;
177  }
178 
179  if (!image || !new_image)
180  {
181  return false;
182  }
183 
184  new_image_ = false;
185 
186  if (image->data.empty())
187  {
188  return false;
189  }
190 
191  Ogre::PixelFormat format = Ogre::PF_R8G8B8;
192  Ogre::Image ogre_image;
193  std::vector<uint8_t> buffer;
194 
195  uint8_t* imageDataPtr = (uint8_t*)&image->data[0];
196  size_t imageDataSize = image->data.size();
197 
198  if (image->encoding == sensor_msgs::image_encodings::RGB8)
199  {
200  format = Ogre::PF_BYTE_RGB;
201  }
202  else if (image->encoding == sensor_msgs::image_encodings::RGBA8)
203  {
204  format = Ogre::PF_BYTE_RGBA;
205  }
206  else if (image->encoding == sensor_msgs::image_encodings::TYPE_8UC4 ||
207  image->encoding == sensor_msgs::image_encodings::TYPE_8SC4 ||
208  image->encoding == sensor_msgs::image_encodings::BGRA8)
209  {
210  format = Ogre::PF_BYTE_BGRA;
211  }
212  else if (image->encoding == sensor_msgs::image_encodings::TYPE_8UC3 ||
213  image->encoding == sensor_msgs::image_encodings::TYPE_8SC3 ||
214  image->encoding == sensor_msgs::image_encodings::BGR8)
215  {
216  format = Ogre::PF_BYTE_BGR;
217  }
218  else if (image->encoding == sensor_msgs::image_encodings::TYPE_8UC1 ||
219  image->encoding == sensor_msgs::image_encodings::TYPE_8SC1 ||
220  image->encoding == sensor_msgs::image_encodings::MONO8)
221  {
222  format = Ogre::PF_BYTE_L;
223  }
224  else if (image->encoding == sensor_msgs::image_encodings::TYPE_16UC1 ||
225  image->encoding == sensor_msgs::image_encodings::TYPE_16SC1 ||
226  image->encoding == sensor_msgs::image_encodings::MONO16)
227  {
228  imageDataSize /= sizeof(uint16_t);
229  normalize<uint16_t>( (uint16_t*)&image->data[0], imageDataSize, buffer );
230  format = Ogre::PF_BYTE_L;
231  imageDataPtr = &buffer[0];
232  }
233  else if (image->encoding.find("bayer") == 0)
234  {
235  format = Ogre::PF_BYTE_L;
236  }
237  else if (image->encoding == sensor_msgs::image_encodings::TYPE_32FC1)
238  {
239  imageDataSize /= sizeof(float);
240  normalize<float>( (float*)&image->data[0], imageDataSize, buffer );
241  format = Ogre::PF_BYTE_L;
242  imageDataPtr = &buffer[0];
243  }
244  else
245  {
246  throw UnsupportedImageEncoding(image->encoding);
247  }
248 
249  width_ = image->width;
250  height_ = image->height;
251 
252  // TODO: Support different steps/strides
253 
254  Ogre::DataStreamPtr pixel_stream;
255  pixel_stream.bind(new Ogre::MemoryDataStream(imageDataPtr, imageDataSize));
256 
257  try
258  {
259  ogre_image.loadRawData(pixel_stream, width_, height_, 1, format, 1, 0);
260  }
261  catch (Ogre::Exception& e)
262  {
263  // TODO: signal error better
264  ROS_ERROR("Error loading image: %s", e.what());
265  return false;
266  }
267 
268  texture_->unload();
269  texture_->loadImage(ogre_image);
270 
271  return true;
272 }
273 
274 void ROSImageTexture::addMessage(const sensor_msgs::Image::ConstPtr& msg)
275 {
276  boost::mutex::scoped_lock lock(mutex_);
277  current_image_ = msg;
278  new_image_ = true;
279 }
280 
281 } // 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 Apr 27 2019 02:33:42