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


cob_vision_utils
Author(s): Jan Fischer
autogenerated on Fri Mar 15 2019 03:10:16