tpimage.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Mårten Björkman (celle@csc.kth.se) 
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
00007  * met:
00008  *
00009  *  1.Redistributions of source code must retain the above copyright
00010  *    notice, this list of conditions and the following disclaimer.
00011  *  2.Redistributions in binary form must reproduce the above
00012  *    copyright notice, this list of conditions and the following
00013  *    disclaimer in the documentation and/or other materials provided
00014  *    with the distribution.  
00015  *  3.The name of Mårten Björkman may not be used to endorse or
00016  *    promote products derived from this software without specific
00017  *    prior written permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00020  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00021  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
00022  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
00023  * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
00024  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00025  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00026  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
00027  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00028  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00029  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00030  */
00031 
00032 #include "tpimage.h"
00033 #include <math.h>
00034 
00035 template<class T>
00036 Image<T>::Image(int w, int h, T *ptr) : width(w), height(h)
00037 {
00038   int extra = 16 / sizeof(T);
00039   if (ptr==NULL) {
00040     img = (T*)malloc((w*h+extra)*sizeof(T));
00041     //img = new T[w*h+extra];
00042     localalloc = true;
00043     //image = (T *)((unsigned long)(img+extra-1) & (unsigned long)(~15));
00044 #if __WORDSIZE == 64
00045     image = (T *)((uint64_t)(img+extra-1) & (uint64_t)(~15));
00046 #else 
00047     image = (T *)((uint32_t)(img+extra-1) & (uint32_t)(~15));
00048 #endif
00049   } else {
00050     img = ptr;
00051     localalloc = false;
00052     image = img;
00053   }
00054 }
00055 
00056 // template class Image<int>;
00057 template<class T>
00058 void Image<T>::SetSize(int w, int h)
00059 {
00060   if (w==width && h==height) 
00061     return;
00062   if (localalloc) 
00063     delete [] img; 
00064   width = w;
00065   height = h;
00066   int extra = 16 / sizeof(T);
00067   img = (T*)malloc((w*h+extra)*sizeof(T));
00068   //  img = new T[w*h+extra];
00069   localalloc = true;
00070   //image  = (T *)((unsigned long)(img+extra-1) & (unsigned long)(~15));
00071 #if __WORDSIZE == 64
00072     image = (T *)((uint64_t)(img+extra-1) & (uint64_t)(~15));
00073 #else 
00074     image = (T *)((uint32_t)(img+extra-1) & (uint32_t)(~15));
00075 #endif
00076 }
00077 
00078 template <class T>
00079 void Image<T>::SetDataAlign(T *ptr, int w, int h)
00080 {
00081   if (localalloc) 
00082     delete [] img; 
00083   width = w;
00084   height = h;
00085   int size = w*h;
00086   int extra = 16 / sizeof(T);
00087   img = (T*)malloc((w*h+extra)*sizeof(T));
00088   // img = new T[w*h+extra];
00089   localalloc = true;
00090   //image  = (T *)((unsigned long)(img+extra-1) & (unsigned long)(~15));
00091 #if __WORDSIZE == 64
00092   image = (T *)((uint64_t)(img+extra-1) & (uint64_t)(~15));
00093 #else 
00094   image = (T *)((uint32_t)(img+extra-1) & (uint32_t)(~15));
00095 #endif
00096   for(int cnt=0;cnt<size;cnt++) 
00097     image[cnt] = ptr[cnt]; 
00098 }
00099 
00100 
00101 template <class T>
00102 void Image<T>::SetDataAlign(const sensor_msgs::Image &img_msg, 
00103                             int w, int h, bool withColor)
00104 {
00105   
00106   /*
00107   // Print info about the image
00108   std::cout << "Image Encoding " << img_msg.encoding << std::endl;
00109   std::cout << "Big Endian " << img_msg.is_bigendian << std::endl;
00110   std::cout << "Step " << img_msg.step << std::endl;
00111   std::cout << "Size of Data array " << img_msg.step * img_msg.height << std::endl;
00112   */
00113 
00114   if (localalloc) 
00115     delete [] img; 
00116   width = w;
00117   height = h;
00118   int size = w*h;
00119   int extra = 16 / sizeof(T);
00120   img = (T*)malloc((w*h+extra)*sizeof(T));
00121   //  img = new T[w*h+extra];
00122   localalloc = true;
00123   //image  = (T *)((unsigned long)(img+extra-1) & (unsigned long)(~15));
00124 #if __WORDSIZE == 64
00125   image = (T *)((uint64_t)(img+extra-1) & (uint64_t)(~15));
00126 #else 
00127   image = (T *)((uint32_t)(img+extra-1) & (uint32_t)(~15));
00128 #endif
00129   
00130   if(!withColor){
00131     for(int cnt=0, pix=0;cnt<size;cnt+=3, pix++){
00132       memcpy(image+cnt, &(img_msg.data.at(sizeof(T)*pix )), sizeof(T));
00133       memcpy(image+cnt+1, &(img_msg.data.at(sizeof(T)*pix )), sizeof(T));
00134       memcpy(image+cnt+2, &(img_msg.data.at(sizeof(T)*pix )), sizeof(T));  
00135     }
00136   } else {
00137     for(int cnt=0;cnt<size;cnt++)
00138       memcpy(image+cnt, &(img_msg.data.at(sizeof(T)*cnt )), sizeof(T));
00139   }
00140 }
00141 
00142 template void Image<float>::SetDataAlign(float *ptr, int w, int h);
00143 template void Image<uint8_t>::SetDataAlign(uint8_t *ptr, int w, int h);
00144 
00145 
00146 template void Image<uint8_t>::SetDataAlign(const sensor_msgs::Image &img_msg, 
00147                                            int w, int h, bool copyToChannels);
00148 template void Image<float>::SetDataAlign(const sensor_msgs::Image &img_msg, 
00149                                          int w, int h, bool copyToChannels);
00150 
00151 template<class T>
00152 bool Image<T>::Load(const char *filename)
00153 {
00154   std::ifstream imagefile;
00155   imagefile.open(filename, std::ios::binary);
00156   if (!imagefile) {
00157     std::cerr << "Error: couldn't find PPM file " << filename << std::endl;
00158     return false;
00159   }
00160   char string[80];
00161   imagefile >> string;
00162   if (strcmp(string,"P2") && strcmp(string,"P5")) {
00163     std::cerr << "Error: " << filename << " is not an PGM file" << std::endl;
00164     return false;
00165   }
00166   char comment[120];
00167   imagefile >> comment[0];
00168   while (comment[0]=='#') {
00169     imagefile.getline(comment, 119, '\n');
00170     imagefile >> comment[0];
00171   }
00172   imagefile.putback(comment[0]);
00173   int w, h, d;
00174   imagefile >> w;
00175   imagefile >> h;
00176   imagefile >> d;
00177   int size = w * h;
00178   if (w!=width || h!=height) {
00179     delete [] img;
00180     width = w;
00181     height = h;
00182     int extra = 16 / sizeof(T);
00183     img = (T*)malloc((w*h+extra)*sizeof(T));
00184     //    img = new T[size+extra];
00185     //image  = (T *)((unsigned long)(img+extra) & (unsigned long)(~15));
00186 #if __WORDSIZE == 64
00187     image = (T *)((uint64_t)(img+extra) & (uint64_t)(~15));
00188 #else 
00189     image = (T *)((uint32_t)(img+extra) & (uint32_t)(~15));
00190 #endif
00191     std::cout << "WARNING: The size of the loaded image was changed" << std::endl;
00192   }
00193   int value;
00194   if (strcmp(string,"P2")) { // not ascii, thus raw
00195     unsigned char *tmp = new unsigned char[size];
00196     imagefile.ignore(1, '\n');
00197     imagefile.read((char*)tmp, size);
00198     for(int cnt=0;cnt<size;cnt++) 
00199       image[cnt] = (T)tmp[cnt];
00200     delete [] tmp;
00201   } else {                   // with ascii
00202     for(int cnt=0;cnt<size;cnt++) {
00203       imagefile >> value;
00204       image[cnt] = (T)value;
00205     }
00206   }
00207   imagefile.close();
00208   return true;
00209 }
00210 
00211 template<class T>
00212 bool Image<T>::LoadRGB(const char *filename)
00213 {
00214   std::ifstream imagefile;
00215   imagefile.open(filename);
00216   if (!imagefile) {
00217     std::cerr << "Error: couldn't find PPM file " << filename << std::endl;
00218     return false;
00219   }
00220   char string[80];
00221   imagefile >> string;
00222   if (strcmp(string,"P3") && strcmp(string,"P6")) {
00223     std::cerr << "Error: " << filename << " is not an PPM file" << std::endl;
00224     return false;
00225   }
00226   char comment[120];
00227   imagefile >> comment[0];
00228   while (comment[0]=='#') {
00229     imagefile.getline(comment, 119, '\n');
00230     //std::cout << comment << std::endl;
00231     imagefile >> comment[0];
00232   }
00233   imagefile.putback(comment[0]);
00234   int w, h, d;
00235   imagefile >> w;
00236   imagefile >> h;
00237   imagefile >> d;
00238   w *= 3;
00239   int size = w * h;
00240   if (w!=width || h!=height) {
00241     delete [] img;
00242     width = w;
00243     height = h;
00244     int extra = 16 / sizeof(T);
00245     img = (T*)malloc((w*h+extra)*sizeof(T));
00246     //    img = new T[size+extra];
00247     //image  = (T *)((unsigned long)(img+extra) & (unsigned long)(~15));
00248 #if __WORDSIZE == 64
00249     image = (T *)((uint64_t)(img+extra) & (uint64_t)(~15));
00250 #else 
00251     image = (T *)((uint32_t)(img+extra) & (uint32_t)(~15));
00252 #endif
00253     std::cout << "WARNING: The size of the loaded image was changed" << std::endl;
00254   }
00255   if (strcmp(string,"P3")) { // not ascii, thus raw
00256     unsigned char *tmp = new unsigned char[size];
00257     imagefile.ignore(1, '\n');
00258     imagefile.read((char*)tmp, size);
00259     for (int cnt=0;cnt<size;cnt+=3) {
00260       image[cnt+0] = (T)tmp[cnt+0];
00261       image[cnt+1] = (T)tmp[cnt+1];
00262       image[cnt+2] = (T)tmp[cnt+2];
00263     }
00264     delete [] tmp;
00265   } else {
00266     int value;
00267     for (int cnt=0;cnt<size;cnt+=3) {
00268       imagefile >> value;
00269       image[cnt+0] = (T)value;
00270       imagefile >> value;
00271       image[cnt+1] = (T)value;
00272       imagefile >> value;
00273       image[cnt+2] = (T)value;
00274     }
00275   }
00276   imagefile.close();
00277   return true;
00278 }
00279 
00280 template<class T>
00281 void Image<T>::Store(const char *filename, bool type, bool ascii) const
00282 {
00283   std::ofstream imagefile;
00284   imagefile.open(filename, std::ios::binary);
00285   if (ascii) 
00286     imagefile << "P2\n";
00287   else 
00288     imagefile << "P5\n";
00289   imagefile << width << " " << height << "\n";
00290   imagefile << "255\n";
00291   int size = width * height;
00292   float small=0, delta=0, large=0;
00293   if (type) { 
00294     small = large = image[0];
00295      for (int cnt=0;cnt<size;cnt++) {
00296       if (small>image[cnt]) small = (float)image[cnt];
00297       if (large<image[cnt]) large = (float)image[cnt];
00298     }
00299     delta = (float)255.0 / (large-small);
00300     if (ascii) {  // with rescale, with ascii
00301       for (int cnt=0;cnt<size;cnt++) {
00302         int value = (int)(delta * ((float)image[cnt]-small));
00303         if (value<0) value = 0;
00304         else if (value>255) value = 255;
00305         imagefile << value;
00306         if ((cnt&15)==15) imagefile << "\n";
00307         else imagefile << " ";
00308       }
00309     } else {      // with rescale, no ascii
00310       unsigned char *tmp = new unsigned char[size];
00311       for (int cnt=0;cnt<size;cnt++) {
00312         assert(!isnan(image[cnt])); //%%%%
00313         int value = (int)(delta * ((float)image[cnt]-small));
00314         if (value<0) tmp[cnt] = 0;
00315         else if (value>255) tmp[cnt] = 255;
00316         tmp[cnt] = (unsigned char)value;
00317       }
00318       imagefile.write((char*)tmp, size);
00319       delete [] tmp;
00320     }
00321   } else {
00322     if (ascii) { // no rescale, with ascii
00323       for(int cnt=0;cnt<size;cnt++) {
00324         int value = (int)image[cnt];
00325         imagefile << value;
00326         if ((cnt&15)==15) imagefile << "\n";
00327         else imagefile << " ";
00328       }
00329     } else {    // no rescale, no ascii
00330       if (typeid(T)==typeid(unsigned char) || typeid(T)==typeid(char))
00331         imagefile.write((char*)image, size);
00332       else {
00333         unsigned char *tmp = new unsigned char[size];
00334         for (int cnt=0;cnt<size;cnt++) 
00335           tmp[cnt] = (unsigned char)image[cnt];
00336         imagefile.write((char*)tmp, size);
00337         delete [] tmp;
00338       }
00339     }
00340   }
00341   imagefile.close();
00342   std::cout << "File " << filename << " saved. ";
00343   if (type) std::cout << "[" << small << "," << large << "]";
00344   std::cout << std::endl;
00345 }
00346 
00347 template<class T>
00348 void Image<T>::StoreRGB(const char *filename) const
00349 {
00350   assert(!(width%3));
00351   std::ofstream imagefile;
00352   imagefile.open(filename);
00353   imagefile << "P3\n"; 
00354   imagefile << width/3 << " " << height << "\n";
00355   imagefile << "255\n";
00356   int size = width * height;
00357 #if 1 // P3
00358   for(int cnt=0;cnt<size;cnt+=3) {
00359     imagefile << (int)image[cnt+0] << " ";
00360     imagefile << (int)image[cnt+1] << " ";
00361     imagefile << (int)image[cnt+2];
00362     if ((cnt%15)==12) imagefile << "\n";
00363     else imagefile << " ";
00364   }
00365 #else // P6
00366   imagefile.write((char*)image, size);
00367 #endif
00368   imagefile.close();
00369   std::cout << "File " << filename << " saved. " << std::endl;
00370 } 
00371 
00372 template<class T>
00373 void Image<T>::StoreYUV(const char *filename) const
00374 {
00375   assert(!(width%2));
00376   std::ofstream imagefile;
00377   imagefile.open(filename);
00378   imagefile << "P3\n"; 
00379   imagefile << width/2 << " " << height << "\n";
00380   imagefile << "255\n";
00381   int size = width * height;
00382   for(int cnt=0;cnt<size;cnt+=4) {
00383     int y1 = (int)image[cnt+3];
00384     int v = (int)image[cnt+2]-128;
00385     int y0 = (int)image[cnt+1];
00386     int u = (int)image[cnt]-128;
00387     int r = (int)(1.0000*y0 - 0.0009*u + 1.1359*v);
00388     int g = (int)(1.0000*y0 - 0.3959*u - 0.5783*v);
00389     int b = (int)(1.0000*y0 + 2.0411*u - 0.0016*v);
00390     r = (r<0 ? 0 : (r>255 ? 255 : r));
00391     g = (g<0 ? 0 : (g>255 ? 255 : g));
00392     b = (b<0 ? 0 : (b>255 ? 255 : b));
00393     imagefile << r << " " << g << " " << b << " ";
00394     r = (int)(1.0000*y1 - 0.0009*u + 1.1359*v);
00395     g = (int)(1.0000*y1 - 0.3959*u - 0.5783*v);
00396     b = (int)(1.0000*y1 + 2.0411*u - 0.0016*v);
00397     r = (r<0 ? 0 : (r>255 ? 255 : r));
00398     g = (g<0 ? 0 : (g>255 ? 255 : g));
00399     b = (b<0 ? 0 : (b>255 ? 255 : b));
00400     imagefile << r << " " << g << " " << b << " ";
00401     if ((cnt%15)==12) imagefile << "\n";
00402     else imagefile << " ";
00403   }
00404   imagefile.close();
00405   std::cout << "File " << filename << " saved. " << std::endl;
00406 }
00407 
00408 template <class T>
00409 void Image<T>::operator=(Image<T> &src)
00410 {
00411   memcpy(image, src.GetData(), sizeof(T) * width * height);
00412 }
00413 
00414 template class Image<int>;
00415 template class Image<uchar>;
00416 template class Image<short>;
00417 template class Image<float>;
00418 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


active_realtime_segmentation
Author(s): Mårten Björkman. Maintained by Jeannette Bohg
autogenerated on Wed Jan 23 2013 15:46:44