ros_image_texture.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <map>
00031 #include <sstream>
00032 #include <algorithm>
00033 #include <iostream>
00034 
00035 #include <boost/algorithm/string/erase.hpp>
00036 #include <boost/foreach.hpp>
00037 
00038 #include <OgreTextureManager.h>
00039 
00040 #include <sensor_msgs/image_encodings.h>
00041 
00042 #include "rviz/image/ros_image_texture.h"
00043 
00044 namespace rviz
00045 {
00046 
00047 ROSImageTexture::ROSImageTexture()
00048 : new_image_(false)
00049 , width_(0)
00050 , height_(0)
00051 , median_frames_(5)
00052 {
00053   empty_image_.load("no_image.png", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
00054 
00055   static uint32_t count = 0;
00056   std::stringstream ss;
00057   ss << "ROSImageTexture" << count++;
00058   texture_ = Ogre::TextureManager::getSingleton().loadImage(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, empty_image_, Ogre::TEX_TYPE_2D, 0);
00059 
00060   setNormalizeFloatImage(true);
00061 }
00062 
00063 ROSImageTexture::~ROSImageTexture()
00064 {
00065   current_image_.reset();
00066 }
00067 
00068 void ROSImageTexture::clear()
00069 {
00070   boost::mutex::scoped_lock lock(mutex_);
00071 
00072   texture_->unload();
00073   texture_->loadImage(empty_image_);
00074 
00075   new_image_ = false;
00076   current_image_.reset();
00077 }
00078 
00079 const sensor_msgs::Image::ConstPtr& ROSImageTexture::getImage()
00080 {
00081   boost::mutex::scoped_lock lock(mutex_);
00082 
00083   return current_image_;
00084 }
00085 
00086 void ROSImageTexture::setMedianFrames( unsigned median_frames )
00087 {
00088   median_frames_ = median_frames;
00089 }
00090 
00091 double ROSImageTexture::updateMedian( std::deque<double>& buffer, double value )
00092 {
00093   //update buffer
00094   while(buffer.size() > median_frames_-1)
00095   {
00096     buffer.pop_back();
00097   }
00098   buffer.push_front(value);
00099   // get median
00100   std::deque<double> buffer2 = buffer;
00101   std::nth_element( buffer2.begin(), buffer2.begin()+buffer2.size()/2, buffer2.end() );
00102   return *( buffer2.begin()+buffer2.size()/2 );
00103 }
00104 
00105 void ROSImageTexture::setNormalizeFloatImage( bool normalize, double min, double max )
00106 {
00107   normalize_ = normalize;
00108   min_ = min;
00109   max_ = max;
00110 }
00111 
00112 
00113 template<typename T>
00114 void ROSImageTexture::normalize( T* image_data, size_t image_data_size, std::vector<uint8_t> &buffer  )
00115 {
00116   // Prepare output buffer
00117   buffer.resize(image_data_size, 0);
00118 
00119   T minValue;
00120   T maxValue;
00121 
00122   if ( normalize_ )
00123   {
00124     T* input_ptr = image_data;
00125     // Find min. and max. pixel value
00126     minValue = std::numeric_limits<T>::max();
00127     maxValue = std::numeric_limits<T>::min();
00128     for( unsigned i = 0; i < image_data_size; ++i )
00129     {
00130       minValue = std::min( minValue, *input_ptr );
00131       maxValue = std::max( maxValue, *input_ptr );
00132       input_ptr++;
00133     }
00134 
00135     if ( median_frames_ > 1 )
00136     {
00137       minValue = updateMedian( min_buffer_, minValue );
00138       maxValue = updateMedian( max_buffer_, maxValue );
00139     }
00140   }
00141   else
00142   {
00143     // set fixed min/max
00144     minValue = min_;
00145     maxValue = max_;
00146   }
00147 
00148   // Rescale floating point image and convert it to 8-bit
00149   double range = maxValue - minValue;
00150   if( range > 0.0 )
00151   {
00152     T* input_ptr = image_data;
00153 
00154     // Pointer to output buffer
00155     uint8_t* output_ptr = &buffer[0];
00156 
00157     // Rescale and quantize
00158     for( size_t i = 0; i < image_data_size; ++i, ++output_ptr, ++input_ptr )
00159     {
00160       double val = (double(*input_ptr - minValue) / range);
00161       if ( val < 0 ) val = 0;
00162       if ( val > 1 ) val = 1;
00163       *output_ptr = val * 255u;
00164     }
00165   }
00166 }
00167 
00168 bool ROSImageTexture::update()
00169 {
00170   sensor_msgs::Image::ConstPtr image;
00171   bool new_image = false;
00172   {
00173     boost::mutex::scoped_lock lock(mutex_);
00174 
00175     image = current_image_;
00176     new_image = new_image_;
00177   }
00178 
00179   if (!image || !new_image)
00180   {
00181     return false;
00182   }
00183 
00184   new_image_ = false;
00185 
00186   if (image->data.empty())
00187   {
00188     return false;
00189   }
00190 
00191   Ogre::PixelFormat format = Ogre::PF_R8G8B8;
00192   Ogre::Image ogre_image;
00193   std::vector<uint8_t> buffer;
00194 
00195   uint8_t* imageDataPtr = (uint8_t*)&image->data[0];
00196   size_t imageDataSize = image->data.size();
00197 
00198   if (image->encoding == sensor_msgs::image_encodings::RGB8)
00199   {
00200     format = Ogre::PF_BYTE_RGB;
00201   }
00202   else if (image->encoding == sensor_msgs::image_encodings::RGBA8)
00203   {
00204     format = Ogre::PF_BYTE_RGBA;
00205   }
00206   else if (image->encoding == sensor_msgs::image_encodings::TYPE_8UC4 ||
00207            image->encoding == sensor_msgs::image_encodings::TYPE_8SC4 ||
00208            image->encoding == sensor_msgs::image_encodings::BGRA8)
00209   {
00210     format = Ogre::PF_BYTE_BGRA;
00211   }
00212   else if (image->encoding == sensor_msgs::image_encodings::TYPE_8UC3 ||
00213            image->encoding == sensor_msgs::image_encodings::TYPE_8SC3 ||
00214            image->encoding == sensor_msgs::image_encodings::BGR8)
00215   {
00216     format = Ogre::PF_BYTE_BGR;
00217   }
00218   else if (image->encoding == sensor_msgs::image_encodings::TYPE_8UC1 ||
00219            image->encoding == sensor_msgs::image_encodings::TYPE_8SC1 ||
00220            image->encoding == sensor_msgs::image_encodings::MONO8)
00221   {
00222     format = Ogre::PF_BYTE_L;
00223   }
00224   else if (image->encoding == sensor_msgs::image_encodings::TYPE_16UC1 ||
00225            image->encoding == sensor_msgs::image_encodings::TYPE_16SC1 ||
00226            image->encoding == sensor_msgs::image_encodings::MONO16)
00227   {
00228     imageDataSize /= sizeof(uint16_t);
00229     normalize<uint16_t>( (uint16_t*)&image->data[0], imageDataSize, buffer );
00230     format = Ogre::PF_BYTE_L;
00231     imageDataPtr = &buffer[0];
00232   }
00233   else if (image->encoding.find("bayer") == 0)
00234   {
00235     format = Ogre::PF_BYTE_L;
00236   }
00237   else if (image->encoding == sensor_msgs::image_encodings::TYPE_32FC1)
00238   {
00239     imageDataSize /= sizeof(float);
00240     normalize<float>( (float*)&image->data[0], imageDataSize, buffer );
00241     format = Ogre::PF_BYTE_L;
00242     imageDataPtr = &buffer[0];
00243   }
00244   else
00245   {
00246     throw UnsupportedImageEncoding(image->encoding);
00247   }
00248 
00249   width_ = image->width;
00250   height_ = image->height;
00251 
00252   // TODO: Support different steps/strides
00253 
00254   Ogre::DataStreamPtr pixel_stream;
00255   pixel_stream.bind(new Ogre::MemoryDataStream(imageDataPtr, imageDataSize));
00256 
00257   try
00258   {
00259     ogre_image.loadRawData(pixel_stream, width_, height_, 1, format, 1, 0);
00260   }
00261   catch (Ogre::Exception& e)
00262   {
00263     // TODO: signal error better
00264     ROS_ERROR("Error loading image: %s", e.what());
00265     return false;
00266   }
00267 
00268   texture_->unload();
00269   texture_->loadImage(ogre_image);
00270 
00271   return true;
00272 }
00273 
00274 void ROSImageTexture::addMessage(const sensor_msgs::Image::ConstPtr& msg)
00275 {
00276   boost::mutex::scoped_lock lock(mutex_);
00277   current_image_ = msg;
00278   new_image_ = true;
00279 }
00280 
00281 } // end of namespace rviz


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:28