VisionUtils.cpp
Go to the documentation of this file.
00001 /****************************************************************
00002 *
00003 * Copyright (c) 2010
00004 *
00005 * Fraunhofer Institute for Manufacturing Engineering
00006 * and Automation (IPA)
00007 *
00008 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009 *
00010 * Project name: care-o-bot
00011 * ROS stack name: cob_driver
00012 * ROS package name: cob_camera_sensors
00013 * Description:
00014 *
00015 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016 *
00017 * Author: Jan Fischer, email:jan.fischer@ipa.fhg.de
00018 * Supervised by: Jan Fischer, email:jan.fischer@ipa.fhg.de
00019 *
00020 * Date of creation: Mai 2008
00021 * ToDo:
00022 *
00023 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024 *
00025 * Redistribution and use in source and binary forms, with or without
00026 * modification, are permitted provided that the following conditions are met:
00027 *
00028 * * Redistributions of source code must retain the above copyright
00029 * notice, this list of conditions and the following disclaimer.
00030 * * Redistributions in binary form must reproduce the above copyright
00031 * notice, this list of conditions and the following disclaimer in the
00032 * documentation and/or other materials provided with the distribution.
00033 * * Neither the name of the Fraunhofer Institute for Manufacturing
00034 * Engineering and Automation (IPA) nor the names of its
00035 * contributors may be used to endorse or promote products derived from
00036 * this software without specific prior written permission.
00037 *
00038 * This program is free software: you can redistribute it and/or modify
00039 * it under the terms of the GNU Lesser General Public License LGPL as
00040 * published by the Free Software Foundation, either version 3 of the
00041 * License, or (at your option) any later version.
00042 *
00043 * This program is distributed in the hope that it will be useful,
00044 * but WITHOUT ANY WARRANTY; without even the implied warranty of
00045 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00046 * GNU Lesser General Public License LGPL for more details.
00047 *
00048 * You should have received a copy of the GNU Lesser General Public
00049 * License LGPL along with this program.
00050 * If not, see <http://www.gnu.org/licenses/>.
00051 *
00052 ****************************************************************/
00053 
00054 #ifdef __LINUX__
00055 #include "cob_vision_utils/VisionUtils.h"
00056 #else
00057 #include "cob_perception_common/cob_vision_utils/common/include/cob_vision_utils/VisionUtils.h"
00058 #endif
00059 
00060 #include <fstream>
00061 
00062 using namespace ipa_Utils;
00063 
00064 cv::Mat ipa_Utils::vstack(const std::vector<cv::Mat> &mats)
00065 {
00066     if (mats.empty())
00067         return cv::Mat();
00068 
00069     // we need to know the total number of rows to create the stacked matrix
00070     int nRows = 0;
00071     int nCols = mats.front().cols;
00072     int datatype = mats.front().type();
00073     std::vector<cv::Mat>::const_iterator it;
00074     for (it = mats.begin(); it != mats.end(); ++it)
00075     {
00076         nRows += it->rows;
00077     }
00078 
00079     // copy data to stacked matrix
00080     int startRow = 0;
00081     int endRow = 0;
00082     cv::Mat stacked(nRows, nCols, datatype);
00083     for (it = mats.begin(); it != mats.end(); ++it)
00084     {
00085         if (it->rows == 0)
00086             continue;
00087 
00088         // make sure all mats have same num of cols and data type
00089         CV_Assert(it->cols == nCols);
00090         CV_Assert(it->type() == datatype);
00091 
00092         startRow = endRow;
00093         endRow = startRow + it->rows;
00094         cv::Mat mat = stacked.rowRange(startRow, endRow);
00095         it->copyTo(mat);
00096     }
00097 
00098     return stacked;
00099 }
00100 
00101 unsigned long ipa_Utils::EvaluatePolynomial(double x, int degree, double* coefficients, double* y)
00102 {
00103         (*y) = coefficients[degree];
00104         for (int i = degree-1; i >= 0; i--)
00105         {
00106                 (*y) *= x;
00107                 (*y) += coefficients[i];
00108         }
00109 
00110         return ipa_Utils::RET_OK;
00111 }
00112 
00113 void ipa_Utils::InitUndistortMap( const cv::Mat& _A, const cv::Mat& _dist_coeffs,
00114                     cv::Mat& _mapxarr, cv::Mat& _mapyarr )
00115 {
00116     uchar* buffer = 0;
00117 
00118         CvMat A = _A;
00119         CvMat dist_coeffs = _dist_coeffs;
00120         CvMat mapxarr = _mapxarr;
00121         CvMat mapyarr = _mapyarr;
00122 
00123     float a[9], k[4];
00124     int coi1 = 0, coi2 = 0;
00125     CvMat mapxstub, *_mapx = &mapxarr;
00126     CvMat mapystub, *_mapy = &mapyarr;
00127     float *mapx, *mapy;
00128     CvMat _a = cvMat( 3, 3, CV_32F, a ), _k;
00129     int mapxstep, mapystep;
00130     int u, v;
00131     float u0, v0, fx, fy, _fx, _fy, k1, k2, p1, p2;
00132     CvSize size;
00133 
00134     _mapx = cvGetMat( _mapx, &mapxstub, &coi1 );
00135     _mapy = cvGetMat( _mapy, &mapystub, &coi2 );
00136    
00137     cvConvert( &A, &_a );
00138     _k = cvMat( dist_coeffs.rows, dist_coeffs.cols,
00139                 CV_MAKETYPE(CV_32F, CV_MAT_CN(dist_coeffs.type)), k );
00140     cvConvert( &dist_coeffs, &_k );
00141 
00142     u0 = a[2]; v0 = a[5];
00143     fx = a[0]; fy = a[4];
00144     _fx = 1.f/fx; _fy = 1.f/fy;
00145     k1 = k[0]; k2 = k[1];
00146     p1 = k[2]; p2 = k[3];
00147 
00148     mapxstep = _mapx->step ? _mapx->step : (1<<30);
00149     mapystep = _mapy->step ? _mapy->step : (1<<30);
00150     mapx = _mapx->data.fl;
00151     mapy = _mapy->data.fl;
00152 
00153     size = cvGetSize(_mapx);
00154     
00155     mapxstep /= sizeof(mapx[0]);
00156     mapystep /= sizeof(mapy[0]);
00157 
00158     for( v = 0; v < size.height; v++, mapx += mapxstep, mapy += mapystep )
00159     {
00160         float y = (v - v0)*_fy;
00161         float y2 = y*y;
00162         float _2p1y = 2*p1*y;
00163         float _3p1y2 = 3*p1*y2;
00164         float p2y2 = p2*y2;
00165 
00166         for( u = 0; u < size.width; u++ )
00167         {
00168             float x = (u - u0)*_fx;
00169             float x2 = x*x;
00170             float r2 = x2 + y2;
00171             float d = 1 + (k1 + k2*r2)*r2;
00172             float _u = fx*(x*(d + _2p1y) + p2y2 + (3*p2)*x2) + u0;
00173             float _v = fy*(y*(d + (2*p2)*x) + _3p1y2 + p1*x2) + v0;
00174             mapx[u] = _u;
00175             mapy[u] = _v;
00176         }
00177     }
00178 
00179     cvFree( &buffer );
00180 }
00181 
00182 unsigned long ipa_Utils::MaskImage(const cv::Mat& source, cv::Mat& dest, const cv::Mat& mask, cv::Mat& destMask, float minMaskThresh, float maxMaskThresh,
00183                                                 int sourceChannel, double sourceMin, double sourceMax)
00184 {
00185         double globalMin = -1;
00186                 double globalMax = -1;
00187 
00188                 double maskMin = -1;
00189                 double maskMax = -1;
00190 
00191                 dest.create(source.rows, source.cols, CV_8UC3); 
00192 
00193                 CV_Assert(sourceChannel >= 1);
00194                 CV_Assert(sourceChannel <= source.channels());
00195 
00197                 CV_Assert(destMask.depth() == CV_8U);
00198                 CV_Assert(destMask.channels() == 3);
00199                 CV_Assert(destMask.cols == source.cols);
00200                 CV_Assert(destMask.rows == source.rows);
00201 
00203                 CV_Assert(mask.depth() == CV_32F);
00204                 CV_Assert(mask.channels() == 1);
00205                 CV_Assert(mask.cols == source.cols);
00206                 CV_Assert(mask.rows == source.rows);
00207 
00210                 if (sourceMin == -1 || sourceMax == -1)
00211                 {
00212                         cv::Mat mixImage(source.rows, source.cols, source.depth(), 1);
00213 
00214                         // Copy channel 2 of source to channel 0 of zSource
00215                         int from_to[] = {sourceChannel-1, 0};
00216 
00217                         cv::mixChannels(&source, 1, &mixImage, 1, from_to, 1);
00218                         cv::minMaxLoc(mixImage, &globalMin, &globalMax);
00219                 }
00220                 else
00221                 {
00222                         std::cerr << "ERROR - OpenCVUtils::MaskImage:" << std::endl;
00223                         std::cerr << "\t ... Parameter sourceChannel ('" << sourceChannel << "') out of range.\n";
00224                         return RET_FAILED;
00225                 }
00226                 
00227                 if (sourceMin == -1) sourceMin = globalMin;
00228                 if (sourceMax == -1) sourceMax = globalMax;
00229 
00230                 cv::minMaxLoc(mask, &maskMin, &maskMax);
00231                 double wMask = maskMax-maskMin;
00232 
00233                 double w = sourceMax-sourceMin;
00234                 int destIndex = 0;
00235                 int sourceIndex = 0;
00236                 int maskIndex = 0;
00237 
00238                 if (source.depth() == CV_32F)
00239                 {
00240                         for(int j=0; j<source.rows; j++)
00241                         {
00242                                 const float* f_source_ptr = source.ptr<float>(j);
00243                                 const float* f_mask_ptr = mask.ptr<float>(j);
00244                                 
00245                                 unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);
00246                                 unsigned char* c_destMask_ptr = destMask.ptr<unsigned char>(j);
00247                 
00248                                 for(int i=0; i<source.cols; i++)
00249                                 {
00250                                         unsigned char V = 0;
00251                                         unsigned char vMask = 0;
00252                                         destIndex = i*3;
00253                                         sourceIndex = i*source.channels();
00254                                         maskIndex = i*mask.channels();
00255 
00256                                         double z = (double)f_source_ptr[sourceIndex + sourceChannel - 1];
00257                                         float maskVal = f_mask_ptr[maskIndex];
00258                                         if (maskVal < maxMaskThresh &&
00259                                                 maskVal > minMaskThresh)
00260                                         {
00261                                                 if (z < sourceMin) 
00262                                                 {
00263                                                         z = sourceMin;
00264                                                         maskVal = (float)maskMin;
00265                                                 }
00266                                                 if (z > sourceMax)
00267                                                 {
00268                                                         z = sourceMax;
00269                                                         maskVal = (float)maskMax;
00270                                                 }
00271                                                 V= (unsigned char)(255.0 * ((z-sourceMin)/w));
00272 
00273                                                 vMask= (unsigned char)(255.0 * ((maskVal-globalMin)/wMask));
00274                                                 
00275                                         }
00276                                         else
00277                                         {
00278                                                 V = 0;
00279                                                 vMask = 0;
00280                                         }
00281 
00282                                         c_dest_ptr[destIndex] = V;
00283                                         c_dest_ptr[destIndex + 1] = V;
00284                                         c_dest_ptr[destIndex + 2] = V;
00285 
00286                                         c_destMask_ptr[destIndex] = vMask;
00287                                         c_destMask_ptr[destIndex + 1] = vMask;
00288                                         c_destMask_ptr[destIndex + 2] = vMask;
00289                                 }
00290                         }
00291                 }
00292                 else if (source.depth() == CV_32S)
00293                 {
00294                         for(int j=0; j<source.rows; j++)
00295                         {
00296                                 const float* f_mask_ptr = mask.ptr<float>(j);
00297                                 const int* i_source_ptr = source.ptr<int>(j);
00298                                 unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);
00299 
00300                                 for(int i=0; i<source.cols; i++)
00301                                 {
00302                                         int iTimes3 = i*3;
00303                                         unsigned char V = 0;
00304                 
00305                                         double z = (double)i_source_ptr[i*source.channels() + sourceChannel - 1];
00306                                 
00307                                         float maskVal = f_mask_ptr[maskIndex];
00308                                         if (maskVal < maxMaskThresh &&
00309                                                 maskVal > minMaskThresh)
00310                                         {
00311                                                 if (z < sourceMin) z = sourceMin;
00312                                                 if (z > sourceMax) z = sourceMax;
00313                                                 V = (unsigned char)(255.0 * ((z-sourceMin)/w));
00314                                         }
00315                                         else
00316                                         {
00317                                                 V = 0;
00318                                         }
00319 
00320                                         c_dest_ptr[iTimes3] = V;
00321                                         c_dest_ptr[iTimes3 + 1] = V;
00322                                         c_dest_ptr[iTimes3 + 2] = V;
00323                                 }
00324                         }
00325                 }
00326                 else if (source.depth() == CV_8U)
00327                 {
00328                         for(int j=0; j<source.rows; j++)
00329                         {
00330                                 const float* f_mask_ptr = mask.ptr<float>(j);
00331                                 const unsigned char* c_source_ptr = source.ptr<unsigned char>(j);
00332                                 unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);
00333 
00334                                 for(int i=0; i<source.cols; i++)
00335                                 {
00336                                         int iTimes3 = i*3;
00337                                         unsigned char V = 0;
00338                 
00339                                         double z = (double)c_source_ptr[i*source.channels() + sourceChannel - 1];
00340         
00341                                         float maskVal = f_mask_ptr[maskIndex];
00342                                         if (maskVal < maxMaskThresh &&
00343                                                 maskVal > minMaskThresh)
00344                                         {
00345                                                 if (z < sourceMin) z = sourceMin;
00346                                                 if (z > sourceMax) z = sourceMax;
00347                                                 V = (unsigned char)(255.0 * ((z-sourceMin)/w));
00348                                         }
00349                                         else
00350                                         {
00351                                                 V = 0;
00352                                         }
00353 
00354                                         c_dest_ptr[iTimes3] = V;
00355                                         c_dest_ptr[iTimes3 + 1] = V;
00356                                         c_dest_ptr[iTimes3 + 2] = V;
00357                                 }
00358                         }
00359                 }
00360                 else
00361                 {
00362                         std::cout << "ERROR - OpenCVUtils::MaskImage:" << std::endl;
00363                         std::cout << "\t ... Image depth of source not supported.\n";
00364                         return RET_FAILED;
00365                 }
00366                 
00367 
00368                 return RET_OK;
00369 }
00370 
00371 unsigned long ipa_Utils::ConvertToShowImage(const cv::Mat& source, cv::Mat& dest, int channel, double min, double max)
00372 {
00373         double globalMin = -1;
00374                 double globalMax = -1;
00375 
00376                 CV_Assert(channel >= 1);
00377                 CV_Assert(channel <= source.channels());
00378 
00379                 dest.create(source.rows, source.cols, CV_8UC3); 
00380 
00382                 cv::Mat mixImage(source.rows, source.cols, source.depth(), 1);
00383 
00384                 // Copy channel 2 of source to channel 0 of zSource
00385                 int from_to[] = {channel-1, 0};
00386 
00387                 cv::mixChannels(&source, 1, &mixImage, 1, from_to, 1);
00388                 cv::minMaxLoc(mixImage, &globalMin, &globalMax);
00389                                 
00390                 if (min == -1) min = globalMin;
00391                 if (max == -1) max = globalMax;
00392 
00393                 double w = max-min;
00394 
00395                 if (source.depth() == CV_32F)
00396                 {
00397                         for(int j=0; j<source.rows; j++)
00398                         {
00399                                 const float* f_source_ptr = source.ptr<float>(j);                               
00400                                 unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);
00401 
00402                                 for(int i=0; i<source.cols; i++)
00403                                 {
00404                                         int iTimes3 = i*3;
00405                 
00406                                         double z = (double)f_source_ptr[i*source.channels() + channel - 1];
00407                                         
00408                                         if (z < min) z = min;
00409                                         if (z > max) z = max;
00410 
00411                                         int V= (int)(255.0 * ((z-min)/w));
00412 
00413                                         c_dest_ptr[iTimes3] = V;
00414                                         c_dest_ptr[iTimes3 + 1] = V;
00415                                         c_dest_ptr[iTimes3 + 2] = V;
00416                                 }
00417                         }
00418                 }
00419                 else if (source.depth() == CV_32S)
00420                 {
00421                         for(int j=0; j<source.rows; j++)
00422                         {
00423                                 const int* i_source_ptr = source.ptr<int>(j);
00424                                 unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);
00425 
00426                                 for(int i=0; i<source.cols; i++)
00427                                 {
00428                                         int iTimes3 = i*3;
00429                 
00430                                         double z = (double)i_source_ptr[i*source.channels() + channel - 1];
00431                                 
00432                                         if (z < min) z = min;
00433                                         if (z > max) z = max;           
00434 
00435                                         int V= (int)(255.0 * ((z-min)/w));
00436 
00437                                         c_dest_ptr[iTimes3] = V;
00438                                         c_dest_ptr[iTimes3 + 1] = V;
00439                                         c_dest_ptr[iTimes3 + 2] = V;
00440                                 }
00441                         }
00442                 }
00443                 else if (source.depth() == CV_8U)
00444                 {
00445                         for(int j=0; j<source.rows; j++)
00446                         {
00447                                 const unsigned char* c_source_ptr = source.ptr<unsigned char>(j);
00448                                 unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);
00449 
00450                                 for(int i=0; i<source.cols; i++)
00451                                 {
00452                                         int iTimes3 = i*3;
00453                 
00454                                         double z = (double)c_source_ptr[i*source.channels() + channel - 1];
00455         
00456                                         if (z < min) z = min;
00457                                         if (z > max) z = max;
00458 
00459                                         int V= (int)(255.0 * ((z-min)/w));
00460 
00461                                         c_dest_ptr[iTimes3] = V;
00462                                         c_dest_ptr[iTimes3 + 1] = V;
00463                                         c_dest_ptr[iTimes3 + 2] = V;
00464                                 }
00465                         }
00466                 }
00467                 else
00468                 {
00469                         std::cout << "ERROR - OpenCVUtils::ConvertToShowImage:" << std::endl;
00470                         std::cout << "\t ... Image depth of source not supported.\n";
00471                         return RET_FAILED;
00472                 }
00473                 
00474 
00475                 return RET_OK;
00476 }
00477 
00478 unsigned long ipa_Utils::FilterByAmplitude(cv::Mat& xyzImage, const cv::Mat& greyImage, cv::Mat* mask, cv::Mat* maskColor, float minMaskThresh, float maxMaskThresh)
00479 {
00480         CV_Assert(xyzImage.type() == CV_32FC3);
00481         CV_Assert(greyImage.type() == CV_32FC1);
00482 
00483         if(mask) mask->create(greyImage.size(), CV_32FC1);
00484         if(maskColor) maskColor->create(greyImage.size(), CV_8UC3);
00485 
00486         int xyzIndex = 0;
00487         int maskColorIndex = 0;
00488         float V = 0;
00489         float vMask = 0;
00490 
00491         unsigned char* c_maskColor_ptr = 0;
00492         float* f_xyz_ptr = 0;
00493         const float* f_grey_ptr = 0;
00494         float* f_mask_ptr = 0;
00495 
00496         for(int j=0; j<xyzImage.rows; j++)
00497         {
00498                 f_xyz_ptr = xyzImage.ptr<float>(j);
00499                 f_grey_ptr = greyImage.ptr<float>(j);
00500                 if(mask) f_mask_ptr = mask->ptr<float>(j);
00501                 if(maskColor) c_maskColor_ptr = maskColor->ptr<unsigned char>(j);
00502 
00503                 for(int i=0; i<xyzImage.cols; i++)
00504                 {
00505                         
00506                         xyzIndex = i*3;
00507                         maskColorIndex = i*3;
00508 
00509                         double z = (double)f_xyz_ptr[xyzIndex + 2];
00510                         float maskVal = f_grey_ptr[i];
00511 
00512                         if(maskColor)
00513                         {
00515                                 if(maskVal>maxMaskThresh)
00516                                 {
00517                                         c_maskColor_ptr[maskColorIndex]= 0;
00518                                         c_maskColor_ptr[maskColorIndex+1]= 0;
00519                                         c_maskColor_ptr[maskColorIndex+2]= 255;
00520                                 }
00521                                 else if(maskVal<minMaskThresh)
00522                                 {
00523                                         c_maskColor_ptr[maskColorIndex]= 0;
00524                                         c_maskColor_ptr[maskColorIndex+1]= 255;
00525                                         c_maskColor_ptr[maskColorIndex+2]= 0;
00526                                 }
00527                                 else if(z<0.3)
00528                                 {
00529                                         c_maskColor_ptr[maskColorIndex]= 255;
00530                                         c_maskColor_ptr[maskColorIndex+1]= 0;
00531                                         c_maskColor_ptr[maskColorIndex+2]= 0;
00532                                 }
00533                                 else
00534                                 {
00535                                         c_maskColor_ptr[maskColorIndex]= 0;
00536                                         c_maskColor_ptr[maskColorIndex+1]= 0;
00537                                         c_maskColor_ptr[maskColorIndex+2]= 0;
00538                                 }
00539                         }
00540 
00541                         if (maskVal < maxMaskThresh &&
00542                                 maskVal > minMaskThresh)
00543                         {
00544                                 vMask = 0;
00545                         }
00546                         else
00547                         {
00548                                 vMask = 1;
00549                                 f_xyz_ptr[xyzIndex] = V;
00550                                 f_xyz_ptr[xyzIndex + 1] = V;
00551                                 f_xyz_ptr[xyzIndex + 2] = V;
00552                         }
00553 
00554                         if(mask)
00555                         {
00556                                 f_mask_ptr[i] = vMask;
00557                         }
00558 
00559                 }
00560         }
00561 
00562         return RET_OK;
00563 }
00564 
00565 unsigned long ipa_Utils::FilterTearOffEdges(cv::Mat& xyzImage, cv::Mat* mask, float piHalfFraction)
00566 {
00568         CV_Assert(xyzImage.type() == CV_32FC3);
00569 
00570         float pi = 3.14159f;
00571         float t_lower =pi/piHalfFraction;
00572         float t_upper = pi - t_lower;
00573 
00574         if(mask)
00575         {
00576                 mask->create(xyzImage.size() , CV_8UC3);
00577                 mask->setTo(0);
00578         }
00579 
00580         for(int row=0; row < xyzImage.rows; row++)
00581         {
00582                 int index_vLeft = -1;
00583                 int index_vMiddle = -1;
00584                 int index_vRight = -1;
00585                 int index_vUp = -1;
00586                 int index_vDown = -1;
00587 
00588                 cv::Vec3f vLeft = cv::Vec3f::all(0);
00589                 cv::Vec3f vMiddle = cv::Vec3f::all(0);
00590                 cv::Vec3f vRight = cv::Vec3f::all(0);
00591                 cv::Vec3f vUp = cv::Vec3f::all(0);
00592                 cv::Vec3f vDown = cv::Vec3f::all(0);
00593 
00594                 cv::Vec3f vDiff = cv::Vec3f::all(0);
00595 
00596                 float* f_image_ptr_RowUp = 0;
00597                 float* f_image_ptr_RowMiddle = 0;
00598                 float* f_image_ptr_RowDown = 0;
00599 
00600                 float dot = -1.f;
00601                 float angle = -1.f;
00602 
00603                 if (row-1 >= 0)
00604                 {
00605                         f_image_ptr_RowUp = xyzImage.ptr<float>(row-1);
00606                 }
00607 
00608                 f_image_ptr_RowMiddle = xyzImage.ptr<float>(row);
00609 
00610                 if (row+1 < xyzImage.rows)
00611                 {
00612                         f_image_ptr_RowDown = xyzImage.ptr<float>(row+1);
00613                 }
00614 
00621                 for(int col=0; col < xyzImage.cols; col++)
00622                 {
00624                         int score = 0;
00625 
00627                         index_vMiddle = col;
00628                         vMiddle[0] = f_image_ptr_RowMiddle[3*index_vMiddle];
00629                         vMiddle[1] = f_image_ptr_RowMiddle[3*index_vMiddle + 1];
00630                         vMiddle[2] = f_image_ptr_RowMiddle[3*index_vMiddle + 2];
00631 
00633                         if (col-1 >= 0)
00634                         {
00635                                 index_vLeft = col-1;
00636                                 vLeft[0] = f_image_ptr_RowMiddle[3*index_vLeft];
00637                                 vLeft[1] = f_image_ptr_RowMiddle[3*index_vLeft + 1];
00638                                 vLeft[2] = f_image_ptr_RowMiddle[3*index_vLeft + 2];
00639                                 vDiff = vLeft - vMiddle;
00640                                 float vLeftNorm = std::sqrt((vLeft[0] * vLeft[0]) + 
00641                                         (vLeft[1] * vLeft[1]) + (vLeft[2] * vLeft[2]));
00642                                 vLeft[0] = vLeft[0]/vLeftNorm;
00643                                 vLeft[1] = vLeft[1]/vLeftNorm;
00644                                 vLeft[2] = vLeft[2]/vLeftNorm;
00645                                 //vLeft.Normalize();
00646                                 float vDiffNorm = std::sqrt((vDiff[0] * vDiff[0]) +
00647                                         (vDiff[1] * vDiff[1]) + (vDiff[2] * vDiff[2]));
00648                                 vDiff[0] = vDiff[0]/vDiffNorm;
00649                                 vDiff[1] = vDiff[1]/vDiffNorm;
00650                                 vDiff[2] = vDiff[2]/vDiffNorm;
00651                                 //vDiff.Normalize();
00652                                 dot = (float)vDiff.ddot(vLeft);
00653                                 //dot = vDiff.Dot(vLeft);
00654                                 angle = (float)std::acos(dot);
00655                                 //angle = IPA_WM_VERSION::Math<float>::ACos( dot );
00656                                 if (angle > t_upper || angle < t_lower)
00657                                 {
00658                                         score++;
00659                                 }
00660                                 else
00661                                 {
00662                                         score--;
00663                                 }
00664                         }
00665 
00667                         if (col+1 < xyzImage.rows)
00668                         {
00669                                 index_vRight = col+1;
00670                                 vRight[0] = f_image_ptr_RowMiddle[3*index_vRight];
00671                                 vRight[1] = f_image_ptr_RowMiddle[3*index_vRight + 1];
00672                                 vRight[2] = f_image_ptr_RowMiddle[3*index_vRight + 2];
00673                                 vDiff = vRight - vMiddle;
00674                                 float vRightNorm = std::sqrt((vRight[0] * vRight[0]) + 
00675                                         (vRight[1] * vRight[1]) + (vRight[2] * vRight[2]));
00676                                 vRight[0] = vRight[0]/vRightNorm;
00677                                 vRight[1] = vRight[1]/vRightNorm;
00678                                 vRight[2] = vRight[2]/vRightNorm;
00679                                 //vRight.Normalize();
00680                                 float vDiffNorm = std::sqrt((vDiff[0] * vDiff[0]) +
00681                                         (vDiff[1] * vDiff[1]) + (vDiff[2] * vDiff[2]));
00682                                 vDiff[0] = vDiff[0]/vDiffNorm;
00683                                 vDiff[1] = vDiff[1]/vDiffNorm;
00684                                 vDiff[2] = vDiff[2]/vDiffNorm;
00685                                 //vDiff.Normalize();
00686                                 dot = (float)vDiff.ddot(vLeft);
00687                                 //dot = vDiff.Dot(vLeft);
00688                                 angle = (float)std::acos(dot);
00689                                 //angle = IPA_WM_VERSION::Math<float>::ACos( dot );
00690                                 if (angle > t_upper || angle < t_lower)
00691                                 {
00692                                         score++;
00693                                 }
00694                                 else
00695                                 {
00696                                         score--;
00697                                 }
00698                         }
00699 
00701                         if (f_image_ptr_RowUp)
00702                         {
00703                                 index_vUp = col;
00704                                 vUp[0] = f_image_ptr_RowUp[3*index_vUp];
00705                                 vUp[1] = f_image_ptr_RowUp[3*index_vUp + 1];
00706                                 vUp[2] = f_image_ptr_RowUp[3*index_vUp + 2];
00707                                 vDiff = vUp - vMiddle;
00708                                 float vUpNorm = std::sqrt((vUp[0] * vUp[0]) + 
00709                                         (vUp[1] * vUp[1]) + (vUp[2] * vUp[2]));
00710                                 vUp[0] = vUp[0]/vUpNorm;
00711                                 vUp[1] = vUp[1]/vUpNorm;
00712                                 vUp[2] = vUp[2]/vUpNorm;
00713                                 //vUp.Normalize();
00714                                 float vDiffNorm = std::sqrt((vDiff[0] * vDiff[0]) +
00715                                         (vDiff[1] * vDiff[1]) + (vDiff[2] * vDiff[2]));
00716                                 vDiff[0] = vDiff[0]/vDiffNorm;
00717                                 vDiff[1] = vDiff[1]/vDiffNorm;
00718                                 vDiff[2] = vDiff[2]/vDiffNorm;
00719                                 //vDiff.Normalize();
00720                                 dot = (float)vDiff.ddot(vLeft);
00721                                 //dot = vDiff.Dot(vLeft);
00722                                 angle = (float)std::acos(dot);
00723                                 //angle = IPA_WM_VERSION::Math<float>::ACos( dot );
00724                                 if (angle > t_upper || angle < t_lower)
00725                                 {
00726                                         score++;
00727                                 }
00728                                 else
00729                                 {
00730                                         score--;
00731                                 }
00732                         }
00733 
00735                         if (f_image_ptr_RowDown)
00736                         {
00737                                 index_vDown = col;
00738                                 vDown[0] = f_image_ptr_RowDown[3*index_vDown];
00739                                 vDown[1] = f_image_ptr_RowDown[3*index_vDown + 1];
00740                                 vDown[2] = f_image_ptr_RowDown[3*index_vDown + 2];
00741                                 float vDownNorm = std::sqrt((vDown[0] * vDown[0]) + 
00742                                         (vDown[1] * vDown[1]) + (vDown[2] * vDown[2]));
00743                                 vDown[0] = vDown[0]/vDownNorm;
00744                                 vDown[1] = vDown[1]/vDownNorm;
00745                                 vDown[2] = vDown[2]/vDownNorm;
00746                                 //vDown.Normalize();
00747                                 float vDiffNorm = std::sqrt((vDiff[0] * vDiff[0]) +
00748                                         (vDiff[1] * vDiff[1]) + (vDiff[2] * vDiff[2]));
00749                                 vDiff[0] = vDiff[0]/vDiffNorm;
00750                                 vDiff[1] = vDiff[1]/vDiffNorm;
00751                                 vDiff[2] = vDiff[2]/vDiffNorm;
00752                                 //vDiff.Normalize();
00753                                 dot = (float)vDiff.ddot(vLeft);
00754                                 //dot = vDiff.Dot(vLeft);
00755                                 angle = (float)std::acos(dot);
00756                                 //angle = IPA_WM_VERSION::Math<float>::ACos( dot );
00757                                 if (angle > t_upper || angle < t_lower)
00758                                 {
00759                                         score++;
00760                                 }
00761                                 else
00762                                 {
00763                                         score--;
00764                                 }
00765                         }
00766 
00767 
00769                         if (score > 0)
00770                         {
00771                                 cv::Vec3b pt(0, 0, 0);
00772                                 if(mask)
00773                                 {
00774                                         mask->at<cv::Vec3b>(row,col)=pt;
00775                                 }
00776 //                              xyzImage.at<cv::Vec3f>(row, col) = pt;
00777                                 for(int i = 0; i < 3; i++)
00778                                         ((float*)xyzImage.ptr(row))[3*col+i] = 0.f;
00779                         }
00780                 }
00781         }
00782 
00783         return ipa_Utils::RET_OK;
00784 }
00785 
00786 unsigned long ipa_Utils::FilterSpeckles(cv::Mat& img, int maxSpeckleSize, 
00787         double maxDiff, cv::Mat& _buf)
00788 {
00789     CV_Assert( img.type() == CV_32FC3 );
00790 
00791     float newVal = 0;
00792     int width = img.cols, height = img.rows, npixels = width*height;
00793     size_t bufSize = npixels*(int)(sizeof(cv::Point_<short>) + sizeof(int) + sizeof(uchar));
00794     if( !_buf.isContinuous() || !_buf.data || _buf.cols*_buf.rows*_buf.elemSize() < bufSize )
00795         _buf.create(1, bufSize, CV_8U);
00796     
00797     uchar* buf = _buf.data;
00798     int i, j, dstep = img.step/sizeof(cv::Vec3f);
00799     int* labels = (int*)buf;
00800     buf += npixels*sizeof(labels[0]);
00801     cv::Point_<short>* wbuf = (cv::Point_<short>*)buf;
00802     buf += npixels*sizeof(wbuf[0]);
00803     uchar* rtype = (uchar*)buf;
00804     int curlabel = 0;
00805     
00806     // clear out label assignments
00807     memset(labels, 0, npixels*sizeof(labels[0]));
00808     
00809     for( i = 0; i < height; i++ )
00810     {
00811                 cv::Vec3f* ds = img.ptr<cv::Vec3f>(i);
00812         int* ls = labels + width*i;
00813         
00814         for( j = 0; j < width; j++ )
00815         {
00816             if( ds[j][2] != newVal )    // not a bad disparity
00817             {
00818                 if( ls[j] )             // has a label, check for bad label
00819                 {  
00820                     if( rtype[ls[j]] ) // small region, zero out disparity
00821                                         {
00822                         ds[j][0] = (float)newVal;
00823                                                 ds[j][1] = (float)newVal;
00824                                                 ds[j][2] = (float)newVal;
00825                                         }
00826                 }
00827                 // no label, assign and propagate
00828                 else
00829                 {
00830                     cv::Point_<short>* ws = wbuf;       // initialize wavefront
00831                     cv::Point_<short> p((short)j, (short)i);    // current pixel
00832                     curlabel++; // next label
00833                     int count = 0;      // current region size
00834                     ls[j] = curlabel;
00835                     
00836                     // wavefront propagation
00837                     while( ws >= wbuf ) // wavefront not empty
00838                     {
00839                         count++;
00840                         // put neighbors onto wavefront
00841                                                 cv::Vec3f* dpp = &img.at<cv::Vec3f>(p.y, p.x);
00842                                                 cv::Vec3f dp = *dpp;
00843                         int* lpp = labels + width*p.y + p.x;
00844                         
00845                         if( p.x < width-1 && !lpp[+1] && dpp[+1][2] != newVal && std::abs(dp[2] - dpp[+1][2]) <= maxDiff )
00846                         {
00847                             lpp[+1] = curlabel;
00848                             *ws++ = cv::Point_<short>(p.x+1, p.y);
00849                         }
00850                         
00851                                                 if( p.x > 0 && !lpp[-1] && dpp[-1][2] != newVal && std::abs(dp[2] - dpp[-1][2]) <= maxDiff )
00852                         {
00853                             lpp[-1] = curlabel;
00854                             *ws++ = cv::Point_<short>(p.x-1, p.y);
00855                         }
00856                         
00857                         if( p.y < height-1 && !lpp[+width] && dpp[+dstep][2] != newVal && std::abs(dp[2] - dpp[+dstep][2]) <= maxDiff )
00858                         {
00859                             lpp[+width] = curlabel;
00860                             *ws++ = cv::Point_<short>(p.x, p.y+1);
00861                         }
00862                         
00863                         if( p.y > 0 && !lpp[-width] && dpp[-dstep][2] != newVal && std::abs(dp[2] - dpp[-dstep][2]) <= maxDiff )
00864                         {
00865                             lpp[-width] = curlabel;
00866                             *ws++ = cv::Point_<short>(p.x, p.y-1);
00867                         }
00868                         
00869                         // pop most recent and propagate
00870                         // NB: could try least recent, maybe better convergence
00871                         p = *--ws;
00872                     }
00873                     
00874                     // assign label type
00875                     if( count <= maxSpeckleSize )       // speckle region
00876                     {
00877                         rtype[ls[j]] = 1;       // small region label
00878                         ds[j][0] = (float)newVal;
00879                                                 ds[j][1] = (float)newVal;
00880                                                 ds[j][2] = (float)newVal;
00881                     }
00882                     else
00883                         rtype[ls[j]] = 0;       // large region label
00884                 }
00885             }
00886         }
00887     }
00888         return ipa_Utils::RET_OK;
00889 } 
00890    
00891 cv::Vec3b ipa_Utils::GrayColorMap(double value, double min,double max)
00892 {
00893     double rgb[3];
00894     max-=min;
00895     value-=min;
00896     rgb[0]=rgb[1]=rgb[2]=(unsigned char)(255*value/max);
00897     return cv::Vec3b(rgb[2], rgb[1], rgb[0]);
00898 }
00899 
00900 cv::Mat ipa_Utils::GetColorcoded(const cv::Mat& img_32F)
00901 {
00902     if (img_32F.empty())
00903         return img_32F;
00904 
00905     double minVal, maxVal;
00906     cv::minMaxLoc(img_32F, &minVal, &maxVal);
00907     return GetColorcoded(img_32F, minVal, maxVal);
00908 }
00909 
00910 cv::Mat ipa_Utils::GetColorcoded(const cv::Mat& img_32F, double min, double max)
00911 {
00912     double H,S,V;
00913     cv::Mat hsvImage(img_32F.size(), CV_8UC3);
00914     int hsvBlue = (int)(180*2/3);
00915     
00916     if (min > max)
00917     {
00918         std::swap(min, max);
00919     }
00920 
00921     double diff = max-min;
00922     if (diff == 0)
00923     {
00924         diff = 1;
00925     }
00926 
00927     bool hsv = false;
00928 
00929     for (int i = 0; i < img_32F.rows; i++)
00930     {
00931 
00932         for (int j = 0; j < img_32F.cols; j++)
00933         {
00934             double val = (double)img_32F.at<float>(i,j);
00935             val = std::max(std::min(max, val), min);
00936             val = ((val-min)/diff);
00937             if (hsv)
00938             {
00939                 if (val == 0)
00940                 {
00941                     H = 0;
00942                     S = 0;
00943                     V = 0;
00944                 }
00945                 else
00946                 {
00947                     H = val * hsvBlue;
00948                     S = 255;
00949                     V = 255;
00950                 }
00951 
00952                 hsvImage.at<cv::Vec3b>(i,j)[0] = (unsigned char) hsvBlue - H;
00953                 hsvImage.at<cv::Vec3b>(i,j)[1] = (unsigned char) S;
00954                 hsvImage.at<cv::Vec3b>(i,j)[2] = (unsigned char) V;
00955             }
00956             else
00957             {
00958                 hsvImage.at<cv::Vec3b>(i,j) = GrayColorMap(1-val, 0, 1);
00959             }
00960         }
00961     }
00962 
00963     if (hsv)
00964         cv::cvtColor(hsvImage, hsvImage, CV_HSV2BGR);
00965 
00966     return hsvImage;
00967 }
00968 
00969 unsigned long ipa_Utils::SaveMat(cv::Mat& mat, std::string filename, int type)
00970 {
00971         
00972 
00973         std::ofstream f(filename.c_str(), std::ios_base::binary);
00974         if(!f.is_open())
00975         {
00976                 std::cerr << "ERROR - ipa_Utils::SaveMat:" << std::endl;
00977                 std::cerr << "\t ... Could not open " << filename << " \n";
00978                 return ipa_Utils::RET_FAILED;
00979         }
00980 
00981         int channels = mat.channels();
00982         
00983         int header[3];
00984         header[0] = mat.rows;
00985         header[1] = mat.cols;
00986         header[2] = channels;
00987 
00988 #ifndef __LINUX__
00989         f.write((char*)header, 3 * sizeof(int));
00990 #else
00991         f.write((char const*)header, 3 * sizeof(int));
00992 #endif
00993 
00994         if (type == CV_32F)
00995         {
00996                 float* ptr = 0;
00997                 for(unsigned int row=0; row<(unsigned int)mat.rows; row++)
00998                 {
00999                         ptr = mat.ptr<float>(row);
01000                         f.write((char*)ptr, channels * mat.cols * sizeof(float));
01001                 }
01002         }
01003         if (type == CV_8U)
01004         {
01005                 unsigned char* ptr = 0;
01006                 for(unsigned int row=0; row<(unsigned int)mat.rows; row++)
01007                 {
01008                         ptr = mat.ptr<unsigned char>(row);
01009                         f.write((char*)ptr, channels * mat.cols * sizeof(unsigned char));
01010                 }
01011         }
01012 
01013         f.close();
01014         return ipa_Utils::RET_OK;
01015 }
01016 
01017 unsigned long ipa_Utils::LoadMat(cv::Mat& mat, std::string filename, int type)
01018 {
01019         size_t file_length = 0;
01020         char *c_string = 0;
01021 
01022         std::ifstream file(filename.c_str(), std::ios_base::binary|std::ios_base::in|std::ios_base::ate);
01023         if(!file.is_open())
01024         {
01025                 std::cerr << "ERROR - ipa_Utils::LoadMat:" << std::endl;
01026                 std::cerr << "\t ... Could not open " << filename << " \n";
01027                 return ipa_Utils::RET_FAILED;
01028         }
01029 
01030         file_length = file.tellg();
01031         file.seekg(0, std::ios_base::beg);
01032         file.clear();
01033 
01034         c_string = new char[file_length];
01035         file.read(c_string, file_length);
01036         
01037         unsigned int rows, cols;
01038         int channels;
01039         rows = ((int*)c_string)[0];
01040         cols = ((int*)c_string)[1];
01041         channels = ((int*)c_string)[2];
01042 
01043         char* c_ptr;
01044 
01045         if (type == CV_32F)
01046         {
01047                 float* f_ptr;
01048                 mat.create(rows, cols, CV_32FC(channels));
01049                 f_ptr = mat.ptr<float>(0);
01050                 c_ptr = &c_string[3 * sizeof(int)];
01051         
01052                 memcpy(f_ptr, c_ptr,  channels * mat.cols * mat.rows * sizeof(float));
01053         }
01054         if (type == CV_8U)
01055         {
01056                 unsigned char* f_ptr;
01057                 mat.create(rows, cols, CV_32FC(channels));
01058                 f_ptr = mat.ptr<unsigned char>(0);
01059                 c_ptr = &c_string[3 * sizeof(int)];
01060         
01061                 memcpy(f_ptr, c_ptr,  channels * mat.cols * mat.rows * sizeof(unsigned char));
01062         }
01063 
01064         file.close();
01065 
01066         delete[] c_string;
01067 
01068         return ipa_Utils::RET_OK;
01069 }
01070 
01071 ipa_Utils::UniqueNumber::UniqueNumber()
01072 {
01073     current=0;
01074 }
01075 
01076 int ipa_Utils::UniqueNumber::operator()() 
01077 {
01078     return current++;
01079 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


cob_vision_utils
Author(s): Jan Fischer
autogenerated on Sat Mar 9 2013 11:24:23