00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
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
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
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
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
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
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
00644 dot = (float)vDiff.ddot(vLeft);
00645
00646 angle = (float)std::acos(dot);
00647
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
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
00678 dot = (float)vDiff.ddot(vLeft);
00679
00680 angle = (float)std::acos(dot);
00681
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
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
00712 dot = (float)vDiff.ddot(vLeft);
00713
00714 angle = (float)std::acos(dot);
00715
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
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
00745 dot = (float)vDiff.ddot(vLeft);
00746
00747 angle = (float)std::acos(dot);
00748
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
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
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 )
00809 {
00810 if( ls[j] )
00811 {
00812 if( rtype[ls[j]] )
00813 {
00814 ds[j][0] = (float)newVal;
00815 ds[j][1] = (float)newVal;
00816 ds[j][2] = (float)newVal;
00817 }
00818 }
00819
00820 else
00821 {
00822 cv::Point_<short>* ws = wbuf;
00823 cv::Point_<short> p((short)j, (short)i);
00824 curlabel++;
00825 int count = 0;
00826 ls[j] = curlabel;
00827
00828
00829 while( ws >= wbuf )
00830 {
00831 count++;
00832
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
00862
00863 p = *--ws;
00864 }
00865
00866
00867 if( count <= maxSpeckleSize )
00868 {
00869 rtype[ls[j]] = 1;
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;
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 }