VisionUtils.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
18 #include <cob_vision_utils/VisionUtils.h>
19 #include <opencv2/core/core.hpp>
20 #include <opencv2/imgproc/imgproc.hpp>
21 #include <fstream>
22 
23 
24 using namespace ipa_Utils;
25 
26 cv::Mat ipa_Utils::vstack(const std::vector<cv::Mat> &mats)
27 {
28  if (mats.empty())
29  return cv::Mat();
30 
31  // we need to know the total number of rows to create the stacked matrix
32  int nRows = 0;
33  int nCols = mats.front().cols;
34  int datatype = mats.front().type();
35  std::vector<cv::Mat>::const_iterator it;
36  for (it = mats.begin(); it != mats.end(); ++it)
37  {
38  nRows += it->rows;
39  }
40 
41  // copy data to stacked matrix
42  int startRow = 0;
43  int endRow = 0;
44  cv::Mat stacked(nRows, nCols, datatype);
45  for (it = mats.begin(); it != mats.end(); ++it)
46  {
47  if (it->rows == 0)
48  continue;
49 
50  // make sure all mats have same num of cols and data type
51  CV_Assert(it->cols == nCols);
52  CV_Assert(it->type() == datatype);
53 
54  startRow = endRow;
55  endRow = startRow + it->rows;
56  cv::Mat mat = stacked.rowRange(startRow, endRow);
57  it->copyTo(mat);
58  }
59 
60  return stacked;
61 }
62 
63 unsigned long ipa_Utils::EvaluatePolynomial(double x, int degree, double* coefficients, double* y)
64 {
65  (*y) = coefficients[degree];
66  for (int i = degree-1; i >= 0; i--)
67  {
68  (*y) *= x;
69  (*y) += coefficients[i];
70  }
71 
72  return ipa_Utils::RET_OK;
73 }
74 
75 unsigned long ipa_Utils::MaskImage(const cv::Mat& source, cv::Mat& dest, const cv::Mat& mask, cv::Mat& destMask, float minMaskThresh, float maxMaskThresh,
76  int sourceChannel, double sourceMin, double sourceMax)
77 {
78  double globalMin = -1;
79  double globalMax = -1;
80 
81  double maskMin = -1;
82  double maskMax = -1;
83 
84  dest.create(source.rows, source.cols, CV_8UC3);
85 
86  CV_Assert(sourceChannel >= 1);
87  CV_Assert(sourceChannel <= source.channels());
88 
90  CV_Assert(destMask.depth() == CV_8U);
91  CV_Assert(destMask.channels() == 3);
92  CV_Assert(destMask.cols == source.cols);
93  CV_Assert(destMask.rows == source.rows);
94 
96  CV_Assert(mask.depth() == CV_32F);
97  CV_Assert(mask.channels() == 1);
98  CV_Assert(mask.cols == source.cols);
99  CV_Assert(mask.rows == source.rows);
100 
103  if (sourceMin == -1 || sourceMax == -1)
104  {
105  cv::Mat mixImage(source.rows, source.cols, source.depth(), 1);
106 
107  // Copy channel 2 of source to channel 0 of zSource
108  int from_to[] = {sourceChannel-1, 0};
109 
110  cv::mixChannels(&source, 1, &mixImage, 1, from_to, 1);
111  cv::minMaxLoc(mixImage, &globalMin, &globalMax);
112  }
113  else
114  {
115  std::cerr << "ERROR - OpenCVUtils::MaskImage:" << std::endl;
116  std::cerr << "\t ... Parameter sourceChannel ('" << sourceChannel << "') out of range.\n";
117  return RET_FAILED;
118  }
119 
120  if (sourceMin == -1) sourceMin = globalMin;
121  if (sourceMax == -1) sourceMax = globalMax;
122 
123  cv::minMaxLoc(mask, &maskMin, &maskMax);
124  double wMask = maskMax-maskMin;
125 
126  double w = sourceMax-sourceMin;
127  int destIndex = 0;
128  int sourceIndex = 0;
129  int maskIndex = 0;
130 
131  if (source.depth() == CV_32F)
132  {
133  for(int j=0; j<source.rows; j++)
134  {
135  const float* f_source_ptr = source.ptr<float>(j);
136  const float* f_mask_ptr = mask.ptr<float>(j);
137 
138  unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);
139  unsigned char* c_destMask_ptr = destMask.ptr<unsigned char>(j);
140 
141  for(int i=0; i<source.cols; i++)
142  {
143  unsigned char V = 0;
144  unsigned char vMask = 0;
145  destIndex = i*3;
146  sourceIndex = i*source.channels();
147  maskIndex = i*mask.channels();
148 
149  double z = (double)f_source_ptr[sourceIndex + sourceChannel - 1];
150  float maskVal = f_mask_ptr[maskIndex];
151  if (maskVal < maxMaskThresh &&
152  maskVal > minMaskThresh)
153  {
154  if (z < sourceMin)
155  {
156  z = sourceMin;
157  maskVal = (float)maskMin;
158  }
159  if (z > sourceMax)
160  {
161  z = sourceMax;
162  maskVal = (float)maskMax;
163  }
164  V= (unsigned char)(255.0 * ((z-sourceMin)/w));
165 
166  vMask= (unsigned char)(255.0 * ((maskVal-globalMin)/wMask));
167 
168  }
169  else
170  {
171  V = 0;
172  vMask = 0;
173  }
174 
175  c_dest_ptr[destIndex] = V;
176  c_dest_ptr[destIndex + 1] = V;
177  c_dest_ptr[destIndex + 2] = V;
178 
179  c_destMask_ptr[destIndex] = vMask;
180  c_destMask_ptr[destIndex + 1] = vMask;
181  c_destMask_ptr[destIndex + 2] = vMask;
182  }
183  }
184  }
185  else if (source.depth() == CV_32S)
186  {
187  for(int j=0; j<source.rows; j++)
188  {
189  const float* f_mask_ptr = mask.ptr<float>(j);
190  const int* i_source_ptr = source.ptr<int>(j);
191  unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);
192 
193  for(int i=0; i<source.cols; i++)
194  {
195  int iTimes3 = i*3;
196  unsigned char V = 0;
197 
198  double z = (double)i_source_ptr[i*source.channels() + sourceChannel - 1];
199 
200  float maskVal = f_mask_ptr[maskIndex];
201  if (maskVal < maxMaskThresh &&
202  maskVal > minMaskThresh)
203  {
204  if (z < sourceMin) z = sourceMin;
205  if (z > sourceMax) z = sourceMax;
206  V = (unsigned char)(255.0 * ((z-sourceMin)/w));
207  }
208  else
209  {
210  V = 0;
211  }
212 
213  c_dest_ptr[iTimes3] = V;
214  c_dest_ptr[iTimes3 + 1] = V;
215  c_dest_ptr[iTimes3 + 2] = V;
216  }
217  }
218  }
219  else if (source.depth() == CV_8U)
220  {
221  for(int j=0; j<source.rows; j++)
222  {
223  const float* f_mask_ptr = mask.ptr<float>(j);
224  const unsigned char* c_source_ptr = source.ptr<unsigned char>(j);
225  unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);
226 
227  for(int i=0; i<source.cols; i++)
228  {
229  int iTimes3 = i*3;
230  unsigned char V = 0;
231 
232  double z = (double)c_source_ptr[i*source.channels() + sourceChannel - 1];
233 
234  float maskVal = f_mask_ptr[maskIndex];
235  if (maskVal < maxMaskThresh &&
236  maskVal > minMaskThresh)
237  {
238  if (z < sourceMin) z = sourceMin;
239  if (z > sourceMax) z = sourceMax;
240  V = (unsigned char)(255.0 * ((z-sourceMin)/w));
241  }
242  else
243  {
244  V = 0;
245  }
246 
247  c_dest_ptr[iTimes3] = V;
248  c_dest_ptr[iTimes3 + 1] = V;
249  c_dest_ptr[iTimes3 + 2] = V;
250  }
251  }
252  }
253  else
254  {
255  std::cout << "ERROR - OpenCVUtils::MaskImage:" << std::endl;
256  std::cout << "\t ... Image depth of source not supported.\n";
257  return RET_FAILED;
258  }
259 
260 
261  return RET_OK;
262 }
263 
264 unsigned long ipa_Utils::ConvertToShowImage(const cv::Mat& source, cv::Mat& dest, int channel, double min, double max)
265 {
266  double globalMin = -1;
267  double globalMax = -1;
268 
269  CV_Assert(channel >= 1);
270  CV_Assert(channel <= source.channels());
271 
272  dest.create(source.rows, source.cols, CV_8UC3);
273 
275  cv::Mat mixImage(source.rows, source.cols, source.depth(), 1);
276 
277  // Copy channel 2 of source to channel 0 of zSource
278  int from_to[] = {channel-1, 0};
279 
280  cv::mixChannels(&source, 1, &mixImage, 1, from_to, 1);
281  cv::minMaxLoc(mixImage, &globalMin, &globalMax);
282 
283  if (min == -1) min = globalMin;
284  if (max == -1) max = globalMax;
285 
286  double w = max-min;
287 
288  if (source.depth() == CV_32F)
289  {
290  for(int j=0; j<source.rows; j++)
291  {
292  const float* f_source_ptr = source.ptr<float>(j);
293  unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);
294 
295  for(int i=0; i<source.cols; i++)
296  {
297  int iTimes3 = i*3;
298 
299  double z = (double)f_source_ptr[i*source.channels() + channel - 1];
300 
301  if (z < min) z = min;
302  if (z > max) z = max;
303 
304  int V= (int)(255.0 * ((z-min)/w));
305 
306  c_dest_ptr[iTimes3] = V;
307  c_dest_ptr[iTimes3 + 1] = V;
308  c_dest_ptr[iTimes3 + 2] = V;
309  }
310  }
311  }
312  else if (source.depth() == CV_32S)
313  {
314  for(int j=0; j<source.rows; j++)
315  {
316  const int* i_source_ptr = source.ptr<int>(j);
317  unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);
318 
319  for(int i=0; i<source.cols; i++)
320  {
321  int iTimes3 = i*3;
322 
323  double z = (double)i_source_ptr[i*source.channels() + (channel - 1)];
324 
325  if (z < min) z = min;
326  if (z > max) z = max;
327 
328  int V= (int)(255.0 * ((z-min)/w));
329 
330  c_dest_ptr[iTimes3] = V;
331  c_dest_ptr[iTimes3 + 1] = V;
332  c_dest_ptr[iTimes3 + 2] = V;
333  }
334  }
335  }
336  else if (source.depth() == CV_8U)
337  {
338  for(int j=0; j<source.rows; j++)
339  {
340  const unsigned char* c_source_ptr = source.ptr<unsigned char>(j);
341  unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);
342 
343  for(int i=0; i<source.cols; i++)
344  {
345  int iTimes3 = i*3;
346 
347  double z = (double)c_source_ptr[i*source.channels() + (channel - 1)];
348 
349  if (z < min) z = min;
350  if (z > max) z = max;
351 
352  int V= (int)(255.0 * ((z-min)/w));
353 
354  c_dest_ptr[iTimes3] = V;
355  c_dest_ptr[iTimes3 + 1] = V;
356  c_dest_ptr[iTimes3 + 2] = V;
357  }
358  }
359  }
360  else if (source.depth() == CV_16U)
361  {
362  for(int j=0; j<source.rows; j++)
363  {
364  const unsigned short* c_source_ptr = source.ptr<unsigned short>(j);
365  unsigned char* c_dest_ptr = dest.ptr<unsigned char>(j);
366 
367  for(int i=0; i<source.cols; i++)
368  {
369  int iTimes3 = i*3;
370 
371  double z = (double)c_source_ptr[i*source.channels() + (channel - 1)];
372 
373  if (z < min) z = min;
374  if (z > max) z = max;
375 
376  int V= (int)(255.0 * ((z-min)/w));
377 
378  c_dest_ptr[iTimes3] = V;
379  c_dest_ptr[iTimes3 + 1] = V;
380  c_dest_ptr[iTimes3 + 2] = V;
381  }
382  }
383  }
384  else
385  {
386  std::cout << "ERROR - OpenCVUtils::ConvertToShowImage:" << std::endl;
387  std::cout << "\t ... Image depth of source not supported.\n";
388  return RET_FAILED;
389  }
390 
391 
392  return RET_OK;
393 }
394 
395 unsigned long ipa_Utils::FilterByAmplitude(cv::Mat& xyzImage, const cv::Mat& greyImage, cv::Mat* mask, cv::Mat* maskColor, float minMaskThresh, float maxMaskThresh)
396 {
397  CV_Assert(xyzImage.type() == CV_32FC3);
398  CV_Assert(greyImage.type() == CV_32FC1);
399 
400  if(mask) mask->create(greyImage.size(), CV_32FC1);
401  if(maskColor) maskColor->create(greyImage.size(), CV_8UC3);
402 
403  int xyzIndex = 0;
404  int maskColorIndex = 0;
405  float V = 0;
406  float vMask = 0;
407 
408  unsigned char* c_maskColor_ptr = 0;
409  float* f_xyz_ptr = 0;
410  const float* f_grey_ptr = 0;
411  float* f_mask_ptr = 0;
412 
413  for(int j=0; j<xyzImage.rows; j++)
414  {
415  f_xyz_ptr = xyzImage.ptr<float>(j);
416  f_grey_ptr = greyImage.ptr<float>(j);
417  if(mask) f_mask_ptr = mask->ptr<float>(j);
418  if(maskColor) c_maskColor_ptr = maskColor->ptr<unsigned char>(j);
419 
420  for(int i=0; i<xyzImage.cols; i++)
421  {
422 
423  xyzIndex = i*3;
424  maskColorIndex = i*3;
425 
426  double z = (double)f_xyz_ptr[xyzIndex + 2];
427  float maskVal = f_grey_ptr[i];
428 
429  if(maskColor)
430  {
432  if(maskVal>maxMaskThresh)
433  {
434  c_maskColor_ptr[maskColorIndex]= 0;
435  c_maskColor_ptr[maskColorIndex+1]= 0;
436  c_maskColor_ptr[maskColorIndex+2]= 255;
437  }
438  else if(maskVal<minMaskThresh)
439  {
440  c_maskColor_ptr[maskColorIndex]= 0;
441  c_maskColor_ptr[maskColorIndex+1]= 255;
442  c_maskColor_ptr[maskColorIndex+2]= 0;
443  }
444  else if(z<0.3)
445  {
446  c_maskColor_ptr[maskColorIndex]= 255;
447  c_maskColor_ptr[maskColorIndex+1]= 0;
448  c_maskColor_ptr[maskColorIndex+2]= 0;
449  }
450  else
451  {
452  c_maskColor_ptr[maskColorIndex]= 0;
453  c_maskColor_ptr[maskColorIndex+1]= 0;
454  c_maskColor_ptr[maskColorIndex+2]= 0;
455  }
456  }
457 
458  if (maskVal < maxMaskThresh &&
459  maskVal > minMaskThresh)
460  {
461  vMask = 0;
462  }
463  else
464  {
465  vMask = 1;
466  f_xyz_ptr[xyzIndex] = V;
467  f_xyz_ptr[xyzIndex + 1] = V;
468  f_xyz_ptr[xyzIndex + 2] = V;
469  }
470 
471  if(mask)
472  {
473  f_mask_ptr[i] = vMask;
474  }
475 
476  }
477  }
478 
479  return RET_OK;
480 }
481 
482 unsigned long ipa_Utils::FilterTearOffEdges(cv::Mat& xyzImage, cv::Mat* mask, float piHalfFraction)
483 {
485  CV_Assert(xyzImage.type() == CV_32FC3);
486 
487  float pi = 3.14159f;
488  float t_lower =pi/piHalfFraction;
489  float t_upper = pi - t_lower;
490 
491  if(mask)
492  {
493  mask->create(xyzImage.size() , CV_8UC3);
494  mask->setTo(0);
495  }
496 
497  for(int row=0; row < xyzImage.rows; row++)
498  {
499  int index_vLeft = -1;
500  int index_vMiddle = -1;
501  int index_vRight = -1;
502  int index_vUp = -1;
503  int index_vDown = -1;
504 
505  cv::Vec3f vLeft = cv::Vec3f::all(0);
506  cv::Vec3f vMiddle = cv::Vec3f::all(0);
507  cv::Vec3f vRight = cv::Vec3f::all(0);
508  cv::Vec3f vUp = cv::Vec3f::all(0);
509  cv::Vec3f vDown = cv::Vec3f::all(0);
510 
511  cv::Vec3f vDiff = cv::Vec3f::all(0);
512 
513  float* f_image_ptr_RowUp = 0;
514  float* f_image_ptr_RowMiddle = 0;
515  float* f_image_ptr_RowDown = 0;
516 
517  float dot = -1.f;
518  float angle = -1.f;
519 
520  if (row-1 >= 0)
521  {
522  f_image_ptr_RowUp = xyzImage.ptr<float>(row-1);
523  }
524 
525  f_image_ptr_RowMiddle = xyzImage.ptr<float>(row);
526 
527  if (row+1 < xyzImage.rows)
528  {
529  f_image_ptr_RowDown = xyzImage.ptr<float>(row+1);
530  }
531 
538  for(int col=0; col < xyzImage.cols; col++)
539  {
541  int score = 0;
542 
544  index_vMiddle = col;
545  vMiddle[0] = f_image_ptr_RowMiddle[3*index_vMiddle];
546  vMiddle[1] = f_image_ptr_RowMiddle[3*index_vMiddle + 1];
547  vMiddle[2] = f_image_ptr_RowMiddle[3*index_vMiddle + 2];
548 
550  if (col-1 >= 0)
551  {
552  index_vLeft = col-1;
553  vLeft[0] = f_image_ptr_RowMiddle[3*index_vLeft];
554  vLeft[1] = f_image_ptr_RowMiddle[3*index_vLeft + 1];
555  vLeft[2] = f_image_ptr_RowMiddle[3*index_vLeft + 2];
556  vDiff = vLeft - vMiddle;
557  float vLeftNorm = std::sqrt((vLeft[0] * vLeft[0]) +
558  (vLeft[1] * vLeft[1]) + (vLeft[2] * vLeft[2]));
559  vLeft[0] = vLeft[0]/vLeftNorm;
560  vLeft[1] = vLeft[1]/vLeftNorm;
561  vLeft[2] = vLeft[2]/vLeftNorm;
562  //vLeft.Normalize();
563  float vDiffNorm = std::sqrt((vDiff[0] * vDiff[0]) +
564  (vDiff[1] * vDiff[1]) + (vDiff[2] * vDiff[2]));
565  vDiff[0] = vDiff[0]/vDiffNorm;
566  vDiff[1] = vDiff[1]/vDiffNorm;
567  vDiff[2] = vDiff[2]/vDiffNorm;
568  //vDiff.Normalize();
569  dot = (float)vDiff.ddot(vLeft);
570  //dot = vDiff.Dot(vLeft);
571  angle = (float)std::acos(dot);
572  //angle = IPA_WM_VERSION::Math<float>::ACos( dot );
573  if (angle > t_upper || angle < t_lower)
574  {
575  score++;
576  }
577  else
578  {
579  score--;
580  }
581  }
582 
584  if (col+1 < xyzImage.rows)
585  {
586  index_vRight = col+1;
587  vRight[0] = f_image_ptr_RowMiddle[3*index_vRight];
588  vRight[1] = f_image_ptr_RowMiddle[3*index_vRight + 1];
589  vRight[2] = f_image_ptr_RowMiddle[3*index_vRight + 2];
590  vDiff = vRight - vMiddle;
591  float vRightNorm = std::sqrt((vRight[0] * vRight[0]) +
592  (vRight[1] * vRight[1]) + (vRight[2] * vRight[2]));
593  vRight[0] = vRight[0]/vRightNorm;
594  vRight[1] = vRight[1]/vRightNorm;
595  vRight[2] = vRight[2]/vRightNorm;
596  //vRight.Normalize();
597  float vDiffNorm = std::sqrt((vDiff[0] * vDiff[0]) +
598  (vDiff[1] * vDiff[1]) + (vDiff[2] * vDiff[2]));
599  vDiff[0] = vDiff[0]/vDiffNorm;
600  vDiff[1] = vDiff[1]/vDiffNorm;
601  vDiff[2] = vDiff[2]/vDiffNorm;
602  //vDiff.Normalize();
603  dot = (float)vDiff.ddot(vLeft);
604  //dot = vDiff.Dot(vLeft);
605  angle = (float)std::acos(dot);
606  //angle = IPA_WM_VERSION::Math<float>::ACos( dot );
607  if (angle > t_upper || angle < t_lower)
608  {
609  score++;
610  }
611  else
612  {
613  score--;
614  }
615  }
616 
618  if (f_image_ptr_RowUp)
619  {
620  index_vUp = col;
621  vUp[0] = f_image_ptr_RowUp[3*index_vUp];
622  vUp[1] = f_image_ptr_RowUp[3*index_vUp + 1];
623  vUp[2] = f_image_ptr_RowUp[3*index_vUp + 2];
624  vDiff = vUp - vMiddle;
625  float vUpNorm = std::sqrt((vUp[0] * vUp[0]) +
626  (vUp[1] * vUp[1]) + (vUp[2] * vUp[2]));
627  vUp[0] = vUp[0]/vUpNorm;
628  vUp[1] = vUp[1]/vUpNorm;
629  vUp[2] = vUp[2]/vUpNorm;
630  //vUp.Normalize();
631  float vDiffNorm = std::sqrt((vDiff[0] * vDiff[0]) +
632  (vDiff[1] * vDiff[1]) + (vDiff[2] * vDiff[2]));
633  vDiff[0] = vDiff[0]/vDiffNorm;
634  vDiff[1] = vDiff[1]/vDiffNorm;
635  vDiff[2] = vDiff[2]/vDiffNorm;
636  //vDiff.Normalize();
637  dot = (float)vDiff.ddot(vLeft);
638  //dot = vDiff.Dot(vLeft);
639  angle = (float)std::acos(dot);
640  //angle = IPA_WM_VERSION::Math<float>::ACos( dot );
641  if (angle > t_upper || angle < t_lower)
642  {
643  score++;
644  }
645  else
646  {
647  score--;
648  }
649  }
650 
652  if (f_image_ptr_RowDown)
653  {
654  index_vDown = col;
655  vDown[0] = f_image_ptr_RowDown[3*index_vDown];
656  vDown[1] = f_image_ptr_RowDown[3*index_vDown + 1];
657  vDown[2] = f_image_ptr_RowDown[3*index_vDown + 2];
658  float vDownNorm = std::sqrt((vDown[0] * vDown[0]) +
659  (vDown[1] * vDown[1]) + (vDown[2] * vDown[2]));
660  vDown[0] = vDown[0]/vDownNorm;
661  vDown[1] = vDown[1]/vDownNorm;
662  vDown[2] = vDown[2]/vDownNorm;
663  //vDown.Normalize();
664  float vDiffNorm = std::sqrt((vDiff[0] * vDiff[0]) +
665  (vDiff[1] * vDiff[1]) + (vDiff[2] * vDiff[2]));
666  vDiff[0] = vDiff[0]/vDiffNorm;
667  vDiff[1] = vDiff[1]/vDiffNorm;
668  vDiff[2] = vDiff[2]/vDiffNorm;
669  //vDiff.Normalize();
670  dot = (float)vDiff.ddot(vLeft);
671  //dot = vDiff.Dot(vLeft);
672  angle = (float)std::acos(dot);
673  //angle = IPA_WM_VERSION::Math<float>::ACos( dot );
674  if (angle > t_upper || angle < t_lower)
675  {
676  score++;
677  }
678  else
679  {
680  score--;
681  }
682  }
683 
684 
686  if (score > 0)
687  {
688  cv::Vec3b pt(0, 0, 0);
689  if(mask)
690  {
691  mask->at<cv::Vec3b>(row,col)=pt;
692  }
693 // xyzImage.at<cv::Vec3f>(row, col) = pt;
694  for(int i = 0; i < 3; i++)
695  ((float*)xyzImage.ptr(row))[3*col+i] = 0.f;
696  }
697  }
698  }
699 
700  return ipa_Utils::RET_OK;
701 }
702 
703 unsigned long ipa_Utils::FilterSpeckles(cv::Mat& img, int maxSpeckleSize,
704  double maxDiff, cv::Mat& _buf)
705 {
706  CV_Assert( img.type() == CV_32FC3 );
707 
708  float newVal = 0;
709  int width = img.cols, height = img.rows, npixels = width*height;
710  size_t bufSize = npixels*(int)(sizeof(cv::Point_<short>) + sizeof(int) + sizeof(uchar));
711  if( !_buf.isContinuous() || !_buf.data || _buf.cols*_buf.rows*_buf.elemSize() < bufSize )
712  _buf.create(1, bufSize, CV_8U);
713 
714  uchar* buf = _buf.data;
715  int i, j, dstep = img.step/sizeof(cv::Vec3f);
716  int* labels = (int*)buf;
717  buf += npixels*sizeof(labels[0]);
718  cv::Point_<short>* wbuf = (cv::Point_<short>*)buf;
719  buf += npixels*sizeof(wbuf[0]);
720  uchar* rtype = (uchar*)buf;
721  int curlabel = 0;
722 
723  // clear out label assignments
724  memset(labels, 0, npixels*sizeof(labels[0]));
725 
726  for( i = 0; i < height; i++ )
727  {
728  cv::Vec3f* ds = img.ptr<cv::Vec3f>(i);
729  int* ls = labels + width*i;
730 
731  for( j = 0; j < width; j++ )
732  {
733  if( ds[j][2] != newVal ) // not a bad disparity
734  {
735  if( ls[j] ) // has a label, check for bad label
736  {
737  if( rtype[ls[j]] ) // small region, zero out disparity
738  {
739  ds[j][0] = (float)newVal;
740  ds[j][1] = (float)newVal;
741  ds[j][2] = (float)newVal;
742  }
743  }
744  // no label, assign and propagate
745  else
746  {
747  cv::Point_<short>* ws = wbuf; // initialize wavefront
748  cv::Point_<short> p((short)j, (short)i); // current pixel
749  curlabel++; // next label
750  int count = 0; // current region size
751  ls[j] = curlabel;
752 
753  // wavefront propagation
754  while( ws >= wbuf ) // wavefront not empty
755  {
756  count++;
757  // put neighbors onto wavefront
758  cv::Vec3f* dpp = &img.at<cv::Vec3f>(p.y, p.x);
759  cv::Vec3f dp = *dpp;
760  int* lpp = labels + width*p.y + p.x;
761 
762  if( p.x < width-1 && !lpp[+1] && dpp[+1][2] != newVal && std::abs(dp[2] - dpp[+1][2]) <= maxDiff )
763  {
764  lpp[+1] = curlabel;
765  *ws++ = cv::Point_<short>(p.x+1, p.y);
766  }
767 
768  if( p.x > 0 && !lpp[-1] && dpp[-1][2] != newVal && std::abs(dp[2] - dpp[-1][2]) <= maxDiff )
769  {
770  lpp[-1] = curlabel;
771  *ws++ = cv::Point_<short>(p.x-1, p.y);
772  }
773 
774  if( p.y < height-1 && !lpp[+width] && dpp[+dstep][2] != newVal && std::abs(dp[2] - dpp[+dstep][2]) <= maxDiff )
775  {
776  lpp[+width] = curlabel;
777  *ws++ = cv::Point_<short>(p.x, p.y+1);
778  }
779 
780  if( p.y > 0 && !lpp[-width] && dpp[-dstep][2] != newVal && std::abs(dp[2] - dpp[-dstep][2]) <= maxDiff )
781  {
782  lpp[-width] = curlabel;
783  *ws++ = cv::Point_<short>(p.x, p.y-1);
784  }
785 
786  // pop most recent and propagate
787  // NB: could try least recent, maybe better convergence
788  p = *--ws;
789  }
790 
791  // assign label type
792  if( count <= maxSpeckleSize ) // speckle region
793  {
794  rtype[ls[j]] = 1; // small region label
795  ds[j][0] = (float)newVal;
796  ds[j][1] = (float)newVal;
797  ds[j][2] = (float)newVal;
798  }
799  else
800  rtype[ls[j]] = 0; // large region label
801  }
802  }
803  }
804  }
805  return ipa_Utils::RET_OK;
806 }
807 
808 cv::Vec3b ipa_Utils::GrayColorMap(double value, double min,double max)
809 {
810  double rgb[3];
811  max-=min;
812  value-=min;
813  rgb[0]=rgb[1]=rgb[2]=(unsigned char)(255*value/max);
814  return cv::Vec3b(rgb[2], rgb[1], rgb[0]);
815 }
816 
817 unsigned long ipa_Utils::SaveMat(cv::Mat& mat, std::string filename, int type)
818 {
819 
820 
821  std::ofstream f(filename.c_str(), std::ios_base::binary);
822  if(!f.is_open())
823  {
824  std::cerr << "ERROR - ipa_Utils::SaveMat:" << std::endl;
825  std::cerr << "\t ... Could not open " << filename << " \n";
826  return ipa_Utils::RET_FAILED;
827  }
828 
829  int channels = mat.channels();
830 
831  int header[3];
832  header[0] = mat.rows;
833  header[1] = mat.cols;
834  header[2] = channels;
835 
836 #ifndef __LINUX__
837  f.write((char*)header, 3 * sizeof(int));
838 #else
839  f.write((char const*)header, 3 * sizeof(int));
840 #endif
841 
842  if (type == CV_32F)
843  {
844  float* ptr = 0;
845  for(unsigned int row=0; row<(unsigned int)mat.rows; row++)
846  {
847  ptr = mat.ptr<float>(row);
848  f.write((char*)ptr, channels * mat.cols * sizeof(float));
849  }
850  }
851  if (type == CV_8U)
852  {
853  unsigned char* ptr = 0;
854  for(unsigned int row=0; row<(unsigned int)mat.rows; row++)
855  {
856  ptr = mat.ptr<unsigned char>(row);
857  f.write((char*)ptr, channels * mat.cols * sizeof(unsigned char));
858  }
859  }
860 
861  f.close();
862  return ipa_Utils::RET_OK;
863 }
864 
865 unsigned long ipa_Utils::LoadMat(cv::Mat& mat, std::string filename, int type)
866 {
867  size_t file_length = 0;
868  char *c_string = 0;
869 
870  std::ifstream file(filename.c_str(), std::ios_base::binary|std::ios_base::in|std::ios_base::ate);
871  if(!file.is_open())
872  {
873  std::cerr << "ERROR - ipa_Utils::LoadMat:" << std::endl;
874  std::cerr << "\t ... Could not open " << filename << " \n";
875  return ipa_Utils::RET_FAILED;
876  }
877 
878  file_length = file.tellg();
879  file.seekg(0, std::ios_base::beg);
880  file.clear();
881 
882  c_string = new char[file_length];
883  file.read(c_string, file_length);
884 
885  unsigned int rows, cols;
886  int channels;
887  rows = ((int*)c_string)[0];
888  cols = ((int*)c_string)[1];
889  channels = ((int*)c_string)[2];
890 
891  char* c_ptr;
892 
893  if (type == CV_32F)
894  {
895  float* f_ptr;
896  mat.create(rows, cols, CV_32FC(channels));
897  f_ptr = mat.ptr<float>(0);
898  c_ptr = &c_string[3 * sizeof(int)];
899 
900  memcpy(f_ptr, c_ptr, channels * mat.cols * mat.rows * sizeof(float));
901  }
902  if (type == CV_8U)
903  {
904  unsigned char* f_ptr;
905  mat.create(rows, cols, CV_32FC(channels));
906  f_ptr = mat.ptr<unsigned char>(0);
907  c_ptr = &c_string[3 * sizeof(int)];
908 
909  memcpy(f_ptr, c_ptr, channels * mat.cols * mat.rows * sizeof(unsigned char));
910  }
911 
912  file.close();
913 
914  delete[] c_string;
915 
916  return ipa_Utils::RET_OK;
917 }
918 
919 ipa_Utils::UniqueNumber::UniqueNumber()
920 {
921  current=0;
922 }
923 
924 int ipa_Utils::UniqueNumber::operator()()
925 {
926  return current++;
927 }
ipa_Utils::vstack
cv::Mat vstack(const std::vector< cv::Mat > &mats)
Definition: VisionUtils.cpp:26
ipa_Utils::ConvertToShowImage
unsigned long ConvertToShowImage(const cv::Mat &source, cv::Mat &dest, int channel=1, double min=-1, double max=-1)
Definition: VisionUtils.cpp:264
f
f
ipa_Utils::SaveMat
unsigned long SaveMat(cv::Mat &mat, std::string filename, int type=CV_32F)
Definition: VisionUtils.cpp:817
ipa_Utils::FilterSpeckles
unsigned long FilterSpeckles(cv::Mat &img, int maxSpeckleSize, double _maxDiff, cv::Mat &_buf)
Description.
Definition: VisionUtils.cpp:703
ipa_Utils::GrayColorMap
cv::Vec3b GrayColorMap(double value, double min, double max)
Definition: VisionUtils.cpp:808
ipa_Utils
Definition: GlobalDefines.h:21
ipa_Utils::UniqueNumber::operator()
int operator()()
Definition: VisionUtils.cpp:924
header
std_msgs::Header * header(M &m)
ipa_Utils::LoadMat
unsigned long LoadMat(cv::Mat &mat, std::string filename, int type=CV_32F)
Definition: VisionUtils.cpp:865
datatype
const char * datatype()
ipa_Utils::MaskImage
unsigned long MaskImage(const cv::Mat &source, cv::Mat &dest, const cv::Mat &mask, cv::Mat &destMask, float minMaskThresh=0, float maxMaskTresh=20000, int sourceChannel=1, double sourceMin=-1, double sourceMax=-1)
Definition: VisionUtils.cpp:75
ipa_Utils::EvaluatePolynomial
unsigned long EvaluatePolynomial(double x, int degree, double *coefficients, double *y)
Definition: VisionUtils.cpp:63
ipa_Utils::UniqueNumber::UniqueNumber
UniqueNumber()
Definition: VisionUtils.cpp:919
ipa_Utils::FilterTearOffEdges
unsigned long FilterTearOffEdges(cv::Mat &xyzImage, cv::Mat *mask, float piHalfFraction=6)
Definition: VisionUtils.cpp:482
ipa_Utils::RET_OK
Everythings OK.
Definition: GlobalDefines.h:26
ipa_Utils::FilterByAmplitude
unsigned long FilterByAmplitude(cv::Mat &xyzImage, const cv::Mat &greyImage, cv::Mat *mask, cv::Mat *maskColor, float minMaskThresh, float maxMaskThresh)
Definition: VisionUtils.cpp:395
ipa_Utils::RET_FAILED
Something went dramatically wrong.
Definition: GlobalDefines.h:27

cob_vision_utils
Author(s): Jan Fischer
autogenerated on Sun Oct 18 2020 13:13:20