utils.h
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 <sensor_msgs/image_encodings.h>
00031 namespace enc = sensor_msgs::image_encodings;
00032 
00033 namespace rgbd_assembler {
00034 
00035   void transformRGB(float val, float &r, float &g, float &b){
00036     union{int intp; float floatp; } a;
00037     a.floatp = val;
00038     int rgb = *reinterpret_cast<int*>(&a.intp);
00039   
00040     r = ((rgb >> 16) & 0xff) / 255.0f;
00041     g = ((rgb >> 8) & 0xff) / 255.0f;
00042     b = (rgb & 0xff) / 255.0f; 
00043   }
00044 
00045   float getRGB( float r, float g, float b){
00046     union{ int intp; float floatp; } a;
00047     int res = (int(r*255) << 16) | (int(g*255) << 8) | int(b*255);
00048     a.intp=res;
00049     float rgb = *(&a.floatp);
00050     return rgb;
00051   }
00052 
00053   
00054   enum Format { INVALID = -1, GRAY = 0, RGB, BGR, RGBA, BGRA };
00055   
00056   Format getFormat(const std::string& encoding)
00057   {
00058     if (encoding == enc::BGR8)   return BGR;
00059     if (encoding == enc::MONO8)  return GRAY;
00060     if (encoding == enc::RGB8)   return RGB;
00061     if (encoding == enc::MONO16) return GRAY;
00062     if (encoding == enc::BGRA8)  return BGRA;
00063     if (encoding == enc::RGBA8)  return RGBA;
00064     
00065     // We don't support conversions to/from other types
00066     return INVALID;
00067   }
00068  
00069   void RGBToHSV(float r, float g, float b,
00070               float &h, float &s, float &v) 
00071 {  
00072   
00073     float maxC = b;
00074     if (maxC < g) maxC = g;
00075     if (maxC < r) maxC = r;
00076     float minC = b;
00077     if (minC > g) minC = g;
00078     if (minC > r) minC = r;
00079     
00080     float delta = maxC - minC;
00081     
00082     v = maxC;
00083     s = 0;
00084     h = 0;
00085     
00086     if (delta == 0) {
00087       return;
00088     } else {
00089       s = delta / maxC;
00090       float dR = 60*(maxC - r)/delta + 180;
00091       float dG = 60*(maxC - g)/delta + 180;
00092       float dB = 60*(maxC - b)/delta + 180;
00093       if (r == maxC)
00094         h = dB - dG;
00095       else if (g == maxC)
00096         h = 120 + dR - dB;
00097       else
00098         h = 240 + dG - dR;
00099     }
00100     
00101     if (h<0)
00102       h+=360;
00103     if (h>=360)
00104       h-=360;
00105   }
00106 
00107 void HSVToRGB(float h, float s, float v,
00108               float &r, float &g, float &b) 
00109   {  
00110     if(s==0){
00111       r = v;
00112       g = v;
00113       b = v;
00114       return;
00115     }
00116     
00117     
00118     float h_tmp = h/60.0f;
00119     int i = floor(h_tmp);
00120     float f = h_tmp - i;
00121     //if(i%2) f = 1 - f;
00122     float p = v * (1-s);
00123     float q = v * (1-s*f);
00124     float t = v * (1-s * (1-f));
00125     
00126 
00127     switch(i){
00128     case 0:
00129       //   0: v, n, m); 
00130       r = v;
00131       g = t;
00132       b = p;
00133       break;
00134     case 1:
00135       //   1: n, v, m); 
00136       r = q;
00137       g = v;
00138       b = p;
00139       break;
00140     case 2:
00141       //    2: (m, v, n) 
00142       r = p;
00143       g = v;
00144       b = t;
00145       break;
00146     case 3:
00147       //       3: (m, n, v); 
00148       r = p;
00149       g = q;
00150       b = v;
00151       break;
00152     case 4:
00153       //       4: (n, m, v)
00154       r = t;
00155       g = p;
00156       b = v;
00157       break;
00158     case 5:
00159       //      5: (v, m, n); 
00160       r = v;
00161       g = p;
00162       b = q;
00163       break;
00164     }
00165   }
00166 
00167 }


rgbd_assembler
Author(s): Jeannette Bohg
autogenerated on Mon Oct 6 2014 12:00:04