float_image_utils.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #include <pcl/pcl_config.h>
00036 
00037 #include <pcl/visualization/common/float_image_utils.h>
00038 #include <cmath>
00039 #include <algorithm>
00040 #include <pcl/pcl_macros.h>
00041 
00042 using std::cout;
00043 using std::cerr;
00044 
00045 void 
00046 pcl::visualization::FloatImageUtils::getColorForFloat (float value, unsigned char& r, unsigned char& g, unsigned char& b) 
00047 {
00048   if (pcl_isinf (value)) 
00049   {
00050     if (value > 0.0f) 
00051     {
00052       r = 150;  g = 150;  b = 200;  // INFINITY
00053       return;
00054     }
00055     r = 150;  g = 200;  b = 150;  // -INFINITY
00056     return;
00057   }
00058   if (!pcl_isfinite (value)) 
00059   {
00060     r = 200;  g = 150;  b = 150;  // -INFINITY
00061     return;
00062   }
00063   
00064   r = g = b = 0;
00065   value *= 10;
00066   if (value <= 1.0) 
00067   {  // black -> purple
00068     b = static_cast<unsigned char> (pcl_lrint(value*200));
00069     r = static_cast<unsigned char> (pcl_lrint(value*120));
00070   }
00071   else if (value <= 2.0) 
00072   {  // purple -> blue
00073     b = static_cast<unsigned char> (200 + pcl_lrint((value-1.0)*55));
00074     r = static_cast<unsigned char> (120 - pcl_lrint((value-1.0)*120));
00075   }
00076   else if (value <= 3.0) 
00077   {  // blue -> turquoise
00078     b = static_cast<unsigned char> (255 - pcl_lrint((value-2.0)*55));
00079     g = static_cast<unsigned char> (pcl_lrint((value-2.0)*200));
00080   }
00081   else if (value <= 4.0) 
00082   {  // turquoise -> green
00083     b = static_cast<unsigned char> (200 - pcl_lrint((value-3.0)*200));
00084     g = static_cast<unsigned char> (200 + pcl_lrint((value-3.0)*55));
00085   }
00086   else if (value <= 5.0) 
00087   {  // green -> greyish green
00088     g = static_cast<unsigned char> (255 - pcl_lrint((value-4.0)*100));
00089     r = static_cast<unsigned char> (pcl_lrint((value-4.0)*120));
00090   }
00091   else if (value <= 6.0) 
00092   { // greyish green -> red
00093     r = static_cast<unsigned char> (100 + pcl_lrint((value-5.0)*155));
00094     g = static_cast<unsigned char> (120 - pcl_lrint((value-5.0)*120));
00095     b = static_cast<unsigned char> (120 - pcl_lrint((value-5.0)*120));
00096   }
00097   else if (value <= 7.0) 
00098   {  // red -> yellow
00099     r = 255;
00100     g = static_cast<unsigned char> (pcl_lrint((value-6.0)*255));
00101   }
00102   else 
00103   {  // yellow -> white
00104     r = 255;
00105     g = 255;
00106     b = static_cast<unsigned char> (pcl_lrint((value-7.0)*255.0/3.0));
00107   }
00108 }
00109 
00110 void 
00111 pcl::visualization::FloatImageUtils::getColorForAngle (float value, unsigned char& r, unsigned char& g, unsigned char& b) 
00112 {
00113   if (pcl_isinf (value)) 
00114   {
00115     if (value > 0.0f) 
00116     {
00117       r = 150;  g = 150;  b = 200;  // INFINITY
00118       return;
00119     }
00120     r = 150;  g = 200;  b = 150;  // -INFINITY
00121     return;
00122   }
00123   if (!pcl_isfinite (value)) 
00124   {
00125     r = 200;  g = 150;  b = 150;  // -INFINITY
00126     return;
00127   }
00128   
00129   r = g = b = 0;
00130   if (value < -M_PI/2.0f) 
00131   {  // black -> blue
00132     b = static_cast<unsigned char> (pcl_lrint(255*(value+float(M_PI))/(float(M_PI)/2.0f)));
00133   }
00134   else if (value <= 0.0f) 
00135   {  // blue -> white
00136     b = 255;
00137     r = g = static_cast<unsigned char> (pcl_lrint(255*(value+float(M_PI/2))/(float(M_PI)/2.0f)));
00138   }
00139   else if (value <= M_PI/2.0f) 
00140   {  // white -> green
00141     g = 255;
00142     r = b = static_cast<unsigned char> (255-pcl_lrint(255*(value)/(float(M_PI)/2.0f)));
00143   }
00144   else 
00145   {  // green -> black
00146     g = static_cast<unsigned char> (255-pcl_lrint(255*(value-M_PI/2.0f)/(float(M_PI)/2.0f)));
00147   }
00148   //cout << 180.0f*value/M_PI<<"deg => "<<(int)r<<", "<<(int)g<<", "<<(int)b<<"\n";
00149 }
00150 
00151 void 
00152 pcl::visualization::FloatImageUtils::getColorForHalfAngle (float value, unsigned char& r, unsigned char& g, unsigned char& b) 
00153 {
00154   getColorForAngle(2.0f*value, r, g, b);
00155 }
00156 
00157 unsigned char* 
00158 pcl::visualization::FloatImageUtils::getVisualImage (const float* float_image, int width, int height, float min_value, float max_value, bool gray_scale) 
00159 {
00160   //MEASURE_FUNCTION_TIME;
00161   
00162   //cout << "Image is of size "<<width<<"x"<<height<<"\n";
00163   int size = width*height;
00164   int arraySize = 3 * size;
00165   unsigned char* data = new unsigned char[arraySize];
00166   unsigned char* dataPtr = data;
00167   
00168   bool recalculateMinValue = pcl_isinf (min_value),
00169        recalculateMaxValue = pcl_isinf (max_value);
00170   if (recalculateMinValue) min_value = std::numeric_limits<float>::infinity ();
00171   if (recalculateMaxValue) max_value = -std::numeric_limits<float>::infinity ();
00172   
00173   if (recalculateMinValue || recalculateMaxValue) 
00174   {
00175     for (int i=0; i<size; ++i) 
00176     {
00177       float value = float_image[i];
00178       if (!pcl_isfinite(value)) continue;
00179       if (recalculateMinValue)  min_value = (std::min)(min_value, value);
00180       if (recalculateMaxValue)  max_value = (std::max)(max_value, value);
00181     }
00182   }
00183   //cout << "min_value is "<<min_value<<" and max_value is "<<max_value<<".\n";
00184   float factor = 1.0f / (max_value-min_value), offset = -min_value;
00185   
00186   for (int i=0; i<size; ++i) 
00187   {
00188     unsigned char& r=*(dataPtr++), & g=*(dataPtr++), & b=*(dataPtr++);
00189     float value = float_image[i];
00190     
00191     if (!pcl_isfinite(value)) 
00192     {
00193       getColorForFloat(value, r, g, b);
00194       continue;
00195     }
00196     
00197     // Normalize value to [0, 1]
00198     value = std::max (0.0f, std::min (1.0f, factor * (value + offset)));
00199     
00200     // Get a color from the value in [0, 1]
00201     if (gray_scale) 
00202     {
00203       r = g = b = static_cast<unsigned char> (pcl_lrint (value * 255));
00204     }
00205     else 
00206     {
00207       getColorForFloat(value, r, g, b);
00208     }
00209     //cout << "Setting pixel "<<i<<" to "<<(int)r<<", "<<(int)g<<", "<<(int)b<<".\n";
00210   }
00211   
00212   return data;
00213 }
00214 
00215 unsigned char* 
00216 pcl::visualization::FloatImageUtils::getVisualImage (const unsigned short* short_image, int width, int height,
00217                                                      unsigned short min_value, unsigned short max_value,
00218                                                      bool gray_scale)
00219 {
00220   //MEASURE_FUNCTION_TIME;
00221   
00222   //cout << "Image is of size "<<width<<"x"<<height<<"\n";
00223   int size = width*height;
00224   int arraySize = 3 * size;
00225   unsigned char* data = new unsigned char[arraySize];
00226   unsigned char* dataPtr = data;
00227   
00228   float factor = 1.0f / float (max_value - min_value), offset = float (-min_value);
00229   
00230   for (int i=0; i<size; ++i) 
00231   {
00232     unsigned char& r=*(dataPtr++), & g=*(dataPtr++), & b=*(dataPtr++);
00233     float value = short_image[i];
00234     
00235     // Normalize value to [0, 1]
00236     value = std::max (0.0f, std::min (1.0f, factor * (value + offset)));
00237     
00238     // Get a color from the value in [0, 1]
00239     if (gray_scale) 
00240     {
00241       r = g = b = static_cast<unsigned char> (pcl_lrint(value*255));
00242     }
00243     else 
00244     {
00245       getColorForFloat(value, r, g, b);
00246     }
00247     //cout << "Setting pixel "<<i<<" to "<<(int)r<<", "<<(int)g<<", "<<(int)b<<".\n";
00248   }
00249   
00250   return data;
00251 }
00252 
00253 unsigned char* 
00254 pcl::visualization::FloatImageUtils::getVisualAngleImage (const float* angle_image, int width, int height) 
00255 {
00256   int size = width*height;
00257   int arraySize = 3 * size;
00258   unsigned char* data = new unsigned char[arraySize];
00259   unsigned char* dataPtr = data;
00260   
00261   for (int i=0; i<size; ++i) 
00262   {
00263     unsigned char& r=*(dataPtr++), & g=*(dataPtr++), & b=*(dataPtr++);
00264     float angle = angle_image[i];
00265     
00266     getColorForAngle(angle, r, g, b);
00267   }
00268   
00269   return data;
00270 }
00271 
00272 unsigned char* 
00273 pcl::visualization::FloatImageUtils::getVisualHalfAngleImage (const float* angle_image, int width, int height) 
00274 {
00275   int size = width*height;
00276   int arraySize = 3 * size;
00277   unsigned char* data = new unsigned char[arraySize];
00278   unsigned char* dataPtr = data;
00279   
00280   for (int i=0; i<size; ++i) 
00281   {
00282     unsigned char& r=*(dataPtr++), & g=*(dataPtr++), & b=*(dataPtr++);
00283     float angle = angle_image[i];
00284     
00285     getColorForHalfAngle(angle, r, g, b);
00286   }
00287   
00288   return data;
00289 }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:24:11