00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
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
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
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
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
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
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
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
00652 dot = (float)vDiff.ddot(vLeft);
00653
00654 angle = (float)std::acos(dot);
00655
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
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
00686 dot = (float)vDiff.ddot(vLeft);
00687
00688 angle = (float)std::acos(dot);
00689
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
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
00720 dot = (float)vDiff.ddot(vLeft);
00721
00722 angle = (float)std::acos(dot);
00723
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
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
00753 dot = (float)vDiff.ddot(vLeft);
00754
00755 angle = (float)std::acos(dot);
00756
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
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
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 )
00817 {
00818 if( ls[j] )
00819 {
00820 if( rtype[ls[j]] )
00821 {
00822 ds[j][0] = (float)newVal;
00823 ds[j][1] = (float)newVal;
00824 ds[j][2] = (float)newVal;
00825 }
00826 }
00827
00828 else
00829 {
00830 cv::Point_<short>* ws = wbuf;
00831 cv::Point_<short> p((short)j, (short)i);
00832 curlabel++;
00833 int count = 0;
00834 ls[j] = curlabel;
00835
00836
00837 while( ws >= wbuf )
00838 {
00839 count++;
00840
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
00870
00871 p = *--ws;
00872 }
00873
00874
00875 if( count <= maxSpeckleSize )
00876 {
00877 rtype[ls[j]] = 1;
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;
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 }
