35 #include <boost/algorithm/string/erase.hpp> 36 #include <boost/foreach.hpp> 38 #include <OgreTextureManager.h> 48 empty_image_.load(
"no_image.png", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
52 static uint32_t count = 0;
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,
69 boost::mutex::scoped_lock lock(
mutex_);
82 boost::mutex::scoped_lock lock(
mutex_);
99 buffer.push_front(value);
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);
114 template <
typename T>
118 buffer.resize(image_data_size, 0);
125 T* input_ptr = image_data;
127 minValue = std::numeric_limits<T>::max();
128 maxValue = std::numeric_limits<T>::min();
129 for (
unsigned i = 0; i < image_data_size; ++i)
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())
139 minValue = std::min(minValue, *input_ptr);
140 maxValue = std::max(maxValue, *input_ptr);
158 double range = maxValue - minValue;
161 T* input_ptr = image_data;
164 uint8_t* output_ptr = &buffer[0];
167 for (
size_t i = 0; i < image_data_size; ++i, ++output_ptr, ++input_ptr)
169 double val = (double(*input_ptr - minValue) / range);
174 *output_ptr = val * 255u;
181 sensor_msgs::Image::ConstPtr image;
182 bool new_image =
false;
184 boost::mutex::scoped_lock lock(
mutex_);
190 if (!image || !new_image)
197 if (image->data.empty())
202 Ogre::PixelFormat format = Ogre::PF_R8G8B8;
203 Ogre::Image ogre_image;
204 std::vector<uint8_t> buffer;
206 uint8_t* imageDataPtr = (uint8_t*)&image->data[0];
207 size_t imageDataSize = image->data.size();
211 format = Ogre::PF_BYTE_RGB;
215 format = Ogre::PF_BYTE_RGBA;
221 format = Ogre::PF_BYTE_BGRA;
227 format = Ogre::PF_BYTE_BGR;
233 format = Ogre::PF_BYTE_L;
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];
244 else if (image->encoding.find(
"bayer") == 0)
246 format = Ogre::PF_BYTE_L;
250 imageDataSize /=
sizeof(float);
251 normalize<float>((
float*)&image->data[0], imageDataSize, buffer);
252 format = Ogre::PF_BYTE_L;
253 imageDataPtr = &buffer[0];
265 Ogre::DataStreamPtr pixel_stream;
266 pixel_stream.bind(
new Ogre::MemoryDataStream(imageDataPtr, imageDataSize));
270 ogre_image.loadRawData(pixel_stream,
width_,
height_, 1, format, 1, 0);
272 catch (Ogre::Exception& e)
275 ROS_ERROR(
"Error loading image: %s", e.what());
287 boost::mutex::scoped_lock lock(
mutex_);
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
std::deque< double > min_buffer_
const std::string TYPE_8UC3
const sensor_msgs::Image::ConstPtr & getImage()
const std::string TYPE_16SC1