util2d.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other cv::Materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "rtabmap/core/util2d.h"
29 
30 #include <rtabmap/utilite/ULogger.h>
31 #include <rtabmap/utilite/UMath.h>
32 #include <rtabmap/utilite/UConversion.h>
33 #include <rtabmap/utilite/UTimer.h>
34 #include <rtabmap/utilite/UStl.h>
35 #include <rtabmap/core/util3d_transforms.h>
36 #include <rtabmap/core/StereoDense.h>
37 #include <opencv2/calib3d/calib3d.hpp>
38 #include <opencv2/imgproc/imgproc.hpp>
39 #include <opencv2/video/tracking.hpp>
40 #include <opencv2/highgui/highgui.hpp>
41 #include <opencv2/imgproc/types_c.h>
42 #include <map>
43 #include <Eigen/Core>
44 
45 #if CV_MAJOR_VERSION >= 3
46 #include <opencv2/photo/photo.hpp>
47 #endif
48 
49 namespace rtabmap
50 {
51 
52 namespace util2d
53 {
54 
55 // SSD: Sum of Squared Differences
56 float ssd(const cv::Mat & windowLeft, const cv::Mat & windowRight)
57 {
58  UASSERT_MSG(windowLeft.type() == CV_8UC1 || windowLeft.type() == CV_32FC1 || windowLeft.type() == CV_16SC2, uFormat("Type=%d", windowLeft.type()).c_str());
59  UASSERT(windowLeft.type() == windowRight.type());
60  UASSERT_MSG(windowLeft.rows == windowRight.rows, uFormat("%d vs %d", windowLeft.rows, windowRight.rows).c_str());
61  UASSERT_MSG(windowLeft.cols == windowRight.cols, uFormat("%d vs %d", windowLeft.cols, windowRight.cols).c_str());
62 
63  float score = 0.0f;
64  for(int v=0; v<windowLeft.rows; ++v)
65  {
66  for(int u=0; u<windowLeft.cols; ++u)
67  {
68  float s = 0.0f;
69  if(windowLeft.type() == CV_8UC1)
70  {
71  s = float(windowLeft.at<unsigned char>(v,u))-float(windowRight.at<unsigned char>(v,u));
72  }
73  else if(windowLeft.type() == CV_32FC1)
74  {
75  s = windowLeft.at<float>(v,u)-windowRight.at<float>(v,u);
76  }
77  else if(windowLeft.type() == CV_16SC2)
78  {
79  float sL = float(windowLeft.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowLeft.at<cv::Vec2s>(v,u)[1])*0.5f;
80  float sR = float(windowRight.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowRight.at<cv::Vec2s>(v,u)[1])*0.5f;
81  s = sL - sR;
82  }
83 
84  score += s*s;
85  }
86  }
87  return score;
88 }
89 
90 // SAD: Sum of Absolute intensity Differences
91 float sad(const cv::Mat & windowLeft, const cv::Mat & windowRight)
92 {
93  UASSERT_MSG(windowLeft.type() == CV_8UC1 || windowLeft.type() == CV_32FC1 || windowLeft.type() == CV_16SC2, uFormat("Type=%d", windowLeft.type()).c_str());
94  UASSERT(windowLeft.type() == windowRight.type());
95  UASSERT_MSG(windowLeft.rows == windowRight.rows, uFormat("%d vs %d", windowLeft.rows, windowRight.rows).c_str());
96  UASSERT_MSG(windowLeft.cols == windowRight.cols, uFormat("%d vs %d", windowLeft.cols, windowRight.cols).c_str());
97 
98  float score = 0.0f;
99  for(int v=0; v<windowLeft.rows; ++v)
100  {
101  for(int u=0; u<windowLeft.cols; ++u)
102  {
103  if(windowLeft.type() == CV_8UC1)
104  {
105  score += fabs(float(windowLeft.at<unsigned char>(v,u))-float(windowRight.at<unsigned char>(v,u)));
106  }
107  else if(windowLeft.type() == CV_32FC1)
108  {
109  score += fabs(windowLeft.at<float>(v,u)-windowRight.at<float>(v,u));
110  }
111  else if(windowLeft.type() == CV_16SC2)
112  {
113  float sL = float(windowLeft.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowLeft.at<cv::Vec2s>(v,u)[1])*0.5f;
114  float sR = float(windowRight.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowRight.at<cv::Vec2s>(v,u)[1])*0.5f;
115  score += fabs(sL - sR);
116  }
117  }
118  }
119  return score;
120 }
121 
122 std::vector<cv::Point2f> calcStereoCorrespondences(
123  const cv::Mat & leftImage,
124  const cv::Mat & rightImage,
125  const std::vector<cv::Point2f> & leftCorners,
126  std::vector<unsigned char> & status,
127  cv::Size winSize,
128  int maxLevel,
129  int iterations,
130  float minDisparityF,
131  float maxDisparityF,
132  bool ssdApproach)
133 {
134  UDEBUG("winSize=(%d,%d)", winSize.width, winSize.height);
135  UDEBUG("maxLevel=%d", maxLevel);
136  UDEBUG("minDisparity=%f", minDisparityF);
137  UDEBUG("maxDisparity=%f", maxDisparityF);
138  UDEBUG("iterations=%d", iterations);
139  UDEBUG("ssdApproach=%d", ssdApproach?1:0);
140 
141  // window should be odd
142  if(winSize.width%2 == 0)
143  {
144  winSize.width+=1;
145  }
146  if(winSize.height%2 == 0)
147  {
148  winSize.height+=1;
149  }
150 
151  cv::Size halfWin((winSize.width-1)/2, (winSize.height-1)/2);
152 
153  UTimer timer;
154  double pyramidTime = 0.0;
155  double disparityTime = 0.0;
156  double subpixelTime = 0.0;
157 
158  std::vector<cv::Point2f> rightCorners(leftCorners.size());
159  std::vector<cv::Mat> leftPyramid, rightPyramid;
160  maxLevel = cv::buildOpticalFlowPyramid( leftImage, leftPyramid, winSize, maxLevel, false);
161  maxLevel = cv::buildOpticalFlowPyramid( rightImage, rightPyramid, winSize, maxLevel, false);
162  pyramidTime = timer.ticks();
163 
164  status = std::vector<unsigned char>(leftCorners.size(), 0);
165  int totalIterations = 0;
166  int noSubPixel = 0;
167  int added = 0;
168  int minDisparity = std::floor(minDisparityF);
169  int maxDisparity = std::floor(maxDisparityF);
170  for(unsigned int i=0; i<leftCorners.size(); ++i)
171  {
172  int oi=0;
173  float bestScore = -1.0f;
174  int bestScoreIndex = -1;
175  int tmpMinDisparity = minDisparity;
176  int tmpMaxDisparity = maxDisparity;
177 
178  int iterations = 0;
179  for(int level=maxLevel; level>=0; --level)
180  {
181  UASSERT(level < (int)leftPyramid.size());
182 
183  cv::Point2i center(int(leftCorners[i].x/float(1<<level)), int(leftCorners[i].y/float(1<<level)));
184 
185  oi=0;
186  bestScore = -1.0f;
187  bestScoreIndex = -1;
188  int localMaxDisparity = -tmpMaxDisparity / (1<<level);
189  int localMinDisparity = -tmpMinDisparity / (1<<level);
190 
191  if(center.x-halfWin.width-(level==0?1:0) >=0 && center.x+halfWin.width+(level==0?1:0) < leftPyramid[level].cols &&
192  center.y-halfWin.height >=0 && center.y+halfWin.height < leftPyramid[level].rows)
193  {
194  cv::Mat windowLeft(leftPyramid[level],
195  cv::Range(center.y-halfWin.height,center.y+halfWin.height+1),
196  cv::Range(center.x-halfWin.width,center.x+halfWin.width+1));
197  int minCol = center.x+localMaxDisparity-halfWin.width-1;
198  if(minCol < 0)
199  {
200  localMaxDisparity -= minCol;
201  }
202 
203  int maxCol = center.x+localMinDisparity+halfWin.width+1;
204  if(maxCol >= leftPyramid[level].cols)
205  {
206  localMinDisparity += maxCol-leftPyramid[level].cols-1;
207  }
208 
209  if(localMinDisparity < localMaxDisparity)
210  {
211  localMaxDisparity = localMinDisparity;
212  }
213  int length = localMinDisparity-localMaxDisparity+1;
214  std::vector<float> scores = std::vector<float>(length, 0.0f);
215 
216  for(int d=localMinDisparity; d>localMaxDisparity; --d)
217  {
218  ++iterations;
219  cv::Mat windowRight(rightPyramid[level],
220  cv::Range(center.y-halfWin.height,center.y+halfWin.height+1),
221  cv::Range(center.x+d-halfWin.width,center.x+d+halfWin.width+1));
222  scores[oi] = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
223  if(scores[oi] > 0 && (bestScore < 0.0f || scores[oi] < bestScore))
224  {
225  bestScoreIndex = oi;
226  bestScore = scores[oi];
227  }
228  ++oi;
229  }
230 
231  if(bestScoreIndex>=0)
232  {
233  if(level>0)
234  {
235  tmpMaxDisparity = tmpMinDisparity+(bestScoreIndex+1)*(1<<level);
236  tmpMaxDisparity+=tmpMaxDisparity%level;
237  if(tmpMaxDisparity > maxDisparity)
238  {
239  tmpMaxDisparity = maxDisparity;
240  }
241  tmpMinDisparity = tmpMinDisparity+(bestScoreIndex-1)*(1<<level);
242  tmpMinDisparity -= tmpMinDisparity%level;
243  if(tmpMinDisparity < minDisparity)
244  {
245  tmpMinDisparity = minDisparity;
246  }
247  }
248  }
249  }
250  }
251  disparityTime+=timer.ticks();
252  totalIterations+=iterations;
253 
254  if(bestScoreIndex>=0)
255  {
256  //subpixel refining
257  int d = -(tmpMinDisparity+bestScoreIndex);
258 
259  cv::Mat windowLeft(winSize, CV_32FC1);
260  cv::Mat windowRight(winSize, CV_32FC1);
261  cv::getRectSubPix(leftPyramid[0],
262  winSize,
263  leftCorners[i],
264  windowLeft,
265  windowLeft.type());
266  if(leftCorners[i].x != float(int(leftCorners[i].x)))
267  {
268  //recompute bestScore if the pt is not integer
269  cv::getRectSubPix(rightPyramid[0],
270  winSize,
271  cv::Point2f(leftCorners[i].x+float(d), leftCorners[i].y),
272  windowRight,
273  windowRight.type());
274  bestScore = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
275  }
276 
277  float xc = leftCorners[i].x+float(d);
278  float vc = bestScore;
279  float step = 0.5f;
280  std::map<float, float> cache;
281  bool reject = false;
282  for(int it=0; it<iterations; ++it)
283  {
284  float x1 = xc-step;
285  float x2 = xc+step;
286  float v1 = uValue(cache, x1, 0.0f);
287  float v2 = uValue(cache, x2, 0.0f);
288  if(v1 == 0.0f)
289  {
290  cv::getRectSubPix(rightPyramid[0],
291  winSize,
292  cv::Point2f(x1, leftCorners[i].y),
293  windowRight,
294  windowRight.type());
295  v1 = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
296  }
297  if(v2 == 0.0f)
298  {
299  cv::getRectSubPix(rightPyramid[0],
300  winSize,
301  cv::Point2f(x2, leftCorners[i].y),
302  windowRight,
303  windowRight.type());
304  v2 = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
305  }
306 
307  float previousXc = xc;
308  float previousVc = vc;
309 
310  xc = v1<vc&&v1<v2?x1:v2<vc&&v2<v1?x2:xc;
311  vc = v1<vc&&v1<v2?v1:v2<vc&&v2<v1?v2:vc;
312 
313  if(previousXc == xc)
314  {
315  step /= 2.0f;
316  }
317  else
318  {
319  cache.insert(std::make_pair(previousXc, previousVc));
320  }
321 
322  if(/*xc < leftCorners[i].x+float(d)-1.0f || xc > leftCorners[i].x+float(d)+1.0f ||*/
323  float(leftCorners[i].x - xc) <= minDisparityF)
324  {
325  reject = true;
326  break;
327  }
328  }
329 
330  rightCorners[i] = cv::Point2f(xc, leftCorners[i].y);
331  status[i] = reject?0:1;
332  if(!reject)
333  {
334  if(leftCorners[i].x+float(d) != xc)
335  {
336  ++noSubPixel;
337  }
338  ++added;
339  }
340  }
341  subpixelTime+=timer.ticks();
342  }
343  UDEBUG("SubPixel=%d/%d added (total=%d)", noSubPixel, added, (int)status.size());
344  UDEBUG("totalIterations=%d", totalIterations);
345  UDEBUG("Time pyramid = %f s", pyramidTime);
346  UDEBUG("Time disparity = %f s", disparityTime);
347  UDEBUG("Time sub-pixel = %f s", subpixelTime);
348 
349  return rightCorners;
350 }
351 
352 typedef float acctype;
353 typedef float itemtype;
354 #define CV_DESCALE(x,n) (((x) + (1 << ((n)-1))) >> (n))
355 
356 //
357 // Adapted from OpenCV cv::calcOpticalFlowPyrLK() to force
358 // only optical flow on x-axis (assuming that prevImg is the left
359 // image and nextImg is the right image):
360 // https://github.com/Itseez/opencv/blob/ddf82d0b154873510802ef75c53e628cd7b2cb13/modules/video/src/lkpyramid.cpp#L1088
361 //
362 // The difference is on this line:
363 // https://github.com/Itseez/opencv/blob/ddf82d0b154873510802ef75c53e628cd7b2cb13/modules/video/src/lkpyramid.cpp#L683-L684
364 // - cv::Point2f delta( (float)((A12*b2 - A22*b1) * D), (float)((A12*b1 - A11*b2) * D));
365 // + cv::Point2f delta( (float)((A12*b2 - A22*b1) * D), 0); //<--- note the 0 for y
366 //
367 void calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputArray _nextImg,
368  cv::InputArray _prevPts, cv::InputOutputArray _nextPts,
369  cv::OutputArray _status, cv::OutputArray _err,
370  cv::Size winSize, int maxLevel,
371  cv::TermCriteria criteria,
372  int flags, double minEigThreshold )
373 {
374  cv::Mat prevPtsMat = _prevPts.getMat();
375  const int derivDepth = cv::DataType<short>::depth;
376 
377  CV_Assert( maxLevel >= 0 && winSize.width > 2 && winSize.height > 2 );
378 
379  int level=0, i, npoints;
380  CV_Assert( (npoints = prevPtsMat.checkVector(2, CV_32F, true)) >= 0 );
381 
382  if( npoints == 0 )
383  {
384  _nextPts.release();
385  _status.release();
386  _err.release();
387  return;
388  }
389 
390  if( !(flags & cv::OPTFLOW_USE_INITIAL_FLOW) )
391  _nextPts.create(prevPtsMat.size(), prevPtsMat.type(), -1, true);
392 
393  cv::Mat nextPtsMat = _nextPts.getMat();
394  CV_Assert( nextPtsMat.checkVector(2, CV_32F, true) == npoints );
395 
396  const cv::Point2f* prevPts = prevPtsMat.ptr<cv::Point2f>();
397  cv::Point2f* nextPts = nextPtsMat.ptr<cv::Point2f>();
398 
399  _status.create((int)npoints, 1, CV_8U, -1, true);
400  cv::Mat statusMat = _status.getMat(), errMat;
401  CV_Assert( statusMat.isContinuous() );
402  uchar* status = statusMat.ptr();
403  float* err = 0;
404 
405  for( i = 0; i < npoints; i++ )
406  status[i] = true;
407 
408  if( _err.needed() )
409  {
410  _err.create((int)npoints, 1, CV_32F, -1, true);
411  errMat = _err.getMat();
412  CV_Assert( errMat.isContinuous() );
413  err = errMat.ptr<float>();
414  }
415 
416  std::vector<cv::Mat> prevPyr, nextPyr;
417  int levels1 = -1;
418  int lvlStep1 = 1;
419  int levels2 = -1;
420  int lvlStep2 = 1;
421 
422 
423  if(_prevImg.kind() != cv::_InputArray::STD_VECTOR_MAT)
424  {
425  //create pyramid
426  maxLevel = cv::buildOpticalFlowPyramid(_prevImg, prevPyr, winSize, maxLevel, true);
427  }
428  else if(_prevImg.kind() == cv::_InputArray::STD_VECTOR_MAT)
429  {
430  _prevImg.getMatVector(prevPyr);
431  }
432 
433  levels1 = int(prevPyr.size()) - 1;
434  CV_Assert(levels1 >= 0);
435 
436  if (levels1 % 2 == 1 && prevPyr[0].channels() * 2 == prevPyr[1].channels() && prevPyr[1].depth() == derivDepth)
437  {
438  lvlStep1 = 2;
439  levels1 /= 2;
440  }
441 
442  // ensure that pyramid has required padding
443  if(levels1 > 0)
444  {
445  cv::Size fullSize;
446  cv::Point ofs;
447  prevPyr[lvlStep1].locateROI(fullSize, ofs);
448  CV_Assert(ofs.x >= winSize.width && ofs.y >= winSize.height
449  && ofs.x + prevPyr[lvlStep1].cols + winSize.width <= fullSize.width
450  && ofs.y + prevPyr[lvlStep1].rows + winSize.height <= fullSize.height);
451  }
452 
453  if(levels1 < maxLevel)
454  maxLevel = levels1;
455 
456  if(_nextImg.kind() != cv::_InputArray::STD_VECTOR_MAT)
457  {
458  //create pyramid
459  maxLevel = cv::buildOpticalFlowPyramid(_nextImg, nextPyr, winSize, maxLevel, false);
460  }
461  else if(_nextImg.kind() == cv::_InputArray::STD_VECTOR_MAT)
462  {
463  _nextImg.getMatVector(nextPyr);
464  }
465 
466  levels2 = int(nextPyr.size()) - 1;
467  CV_Assert(levels2 >= 0);
468 
469  if (levels2 % 2 == 1 && nextPyr[0].channels() * 2 == nextPyr[1].channels() && nextPyr[1].depth() == derivDepth)
470  {
471  lvlStep2 = 2;
472  levels2 /= 2;
473  }
474 
475  // ensure that pyramid has required padding
476  if(levels2 > 0)
477  {
478  cv::Size fullSize;
479  cv::Point ofs;
480  nextPyr[lvlStep2].locateROI(fullSize, ofs);
481  CV_Assert(ofs.x >= winSize.width && ofs.y >= winSize.height
482  && ofs.x + nextPyr[lvlStep2].cols + winSize.width <= fullSize.width
483  && ofs.y + nextPyr[lvlStep2].rows + winSize.height <= fullSize.height);
484  }
485 
486  if(levels2 < maxLevel)
487  maxLevel = levels2;
488 
489  if( (criteria.type & cv::TermCriteria::COUNT) == 0 )
490  criteria.maxCount = 30;
491  else
492  criteria.maxCount = std::min(std::max(criteria.maxCount, 0), 100);
493  if( (criteria.type & cv::TermCriteria::EPS) == 0 )
494  criteria.epsilon = 0.01;
495  else
496  criteria.epsilon = std::min(std::max(criteria.epsilon, 0.), 10.);
497  criteria.epsilon *= criteria.epsilon;
498 
499  // for all pyramids
500  for( level = maxLevel; level >= 0; level-- )
501  {
502  cv::Mat derivI = prevPyr[level * lvlStep1 + 1];
503 
504  CV_Assert(prevPyr[level * lvlStep1].size() == nextPyr[level * lvlStep2].size());
505  CV_Assert(prevPyr[level * lvlStep1].type() == nextPyr[level * lvlStep2].type());
506 
507  const cv::Mat & prevImg = prevPyr[level * lvlStep1];
508  const cv::Mat & prevDeriv = derivI;
509  const cv::Mat & nextImg = nextPyr[level * lvlStep2];
510 
511  // for all corners
512  {
513  cv::Point2f halfWin((winSize.width-1)*0.5f, (winSize.height-1)*0.5f);
514  const cv::Mat& I = prevImg;
515  const cv::Mat& J = nextImg;
516  const cv::Mat& derivI = prevDeriv;
517 
518  int j, cn = I.channels(), cn2 = cn*2;
519  cv::AutoBuffer<short> _buf(winSize.area()*(cn + cn2));
520  int derivDepth = cv::DataType<short>::depth;
521 
522  cv::Mat IWinBuf(winSize, CV_MAKETYPE(derivDepth, cn), (short*)_buf);
523  cv::Mat derivIWinBuf(winSize, CV_MAKETYPE(derivDepth, cn2), (short*)_buf + winSize.area()*cn);
524 
525  for( int ptidx = 0; ptidx < npoints; ptidx++ )
526  {
527  cv::Point2f prevPt = prevPts[ptidx]*(float)(1./(1 << level));
528  cv::Point2f nextPt;
529  if( level == maxLevel )
530  {
531  if( flags & cv::OPTFLOW_USE_INITIAL_FLOW )
532  nextPt = nextPts[ptidx]*(float)(1./(1 << level));
533  else
534  nextPt = prevPt;
535  }
536  else
537  nextPt = nextPts[ptidx]*2.f;
538  nextPts[ptidx] = nextPt;
539 
540  cv::Point2i iprevPt, inextPt;
541  prevPt -= halfWin;
542  iprevPt.x = cvFloor(prevPt.x);
543  iprevPt.y = cvFloor(prevPt.y);
544 
545  if( iprevPt.x < -winSize.width || iprevPt.x >= derivI.cols ||
546  iprevPt.y < -winSize.height || iprevPt.y >= derivI.rows )
547  {
548  if( level == 0 )
549  {
550  if( status )
551  status[ptidx] = false;
552  if( err )
553  err[ptidx] = 0;
554  }
555  continue;
556  }
557 
558  float a = prevPt.x - iprevPt.x;
559  float b = prevPt.y - iprevPt.y;
560  const int W_BITS = 14, W_BITS1 = 14;
561  const float FLT_SCALE = 1.f/(1 << 20);
562  int iw00 = cvRound((1.f - a)*(1.f - b)*(1 << W_BITS));
563  int iw01 = cvRound(a*(1.f - b)*(1 << W_BITS));
564  int iw10 = cvRound((1.f - a)*b*(1 << W_BITS));
565  int iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
566 
567  int dstep = (int)(derivI.step/derivI.elemSize1());
568  int stepI = (int)(I.step/I.elemSize1());
569  int stepJ = (int)(J.step/J.elemSize1());
570  acctype iA11 = 0, iA12 = 0, iA22 = 0;
571  float A11, A12, A22;
572 
573  // extract the patch from the first image, compute covariation cv::Matrix of derivatives
574  int x, y;
575  for( y = 0; y < winSize.height; y++ )
576  {
577  const uchar* src = I.ptr() + (y + iprevPt.y)*stepI + iprevPt.x*cn;
578  const short* dsrc = derivI.ptr<short>() + (y + iprevPt.y)*dstep + iprevPt.x*cn2;
579 
580  short* Iptr = IWinBuf.ptr<short>(y);
581  short* dIptr = derivIWinBuf.ptr<short>(y);
582 
583  x = 0;
584 
585  for( ; x < winSize.width*cn; x++, dsrc += 2, dIptr += 2 )
586  {
587  int ival = CV_DESCALE(src[x]*iw00 + src[x+cn]*iw01 +
588  src[x+stepI]*iw10 + src[x+stepI+cn]*iw11, W_BITS1-5);
589  int ixval = CV_DESCALE(dsrc[0]*iw00 + dsrc[cn2]*iw01 +
590  dsrc[dstep]*iw10 + dsrc[dstep+cn2]*iw11, W_BITS1);
591  int iyval = CV_DESCALE(dsrc[1]*iw00 + dsrc[cn2+1]*iw01 + dsrc[dstep+1]*iw10 +
592  dsrc[dstep+cn2+1]*iw11, W_BITS1);
593 
594  Iptr[x] = (short)ival;
595  dIptr[0] = (short)ixval;
596  dIptr[1] = (short)iyval;
597 
598  iA11 += (itemtype)(ixval*ixval);
599  iA12 += (itemtype)(ixval*iyval);
600  iA22 += (itemtype)(iyval*iyval);
601  }
602  }
603 
604  A11 = iA11*FLT_SCALE;
605  A12 = iA12*FLT_SCALE;
606  A22 = iA22*FLT_SCALE;
607 
608  float D = A11*A22 - A12*A12;
609  float minEig = (A22 + A11 - std::sqrt((A11-A22)*(A11-A22) +
610  4.f*A12*A12))/(2*winSize.width*winSize.height);
611 
612  if( err && (flags & cv::OPTFLOW_LK_GET_MIN_EIGENVALS) != 0 )
613  err[ptidx] = (float)minEig;
614 
615  if( minEig < minEigThreshold || D < FLT_EPSILON )
616  {
617  if( level == 0 && status )
618  status[ptidx] = false;
619  continue;
620  }
621 
622  D = 1.f/D;
623 
624  nextPt -= halfWin;
625  cv::Point2f prevDelta;
626 
627  for( j = 0; j < criteria.maxCount; j++ )
628  {
629  inextPt.x = cvFloor(nextPt.x);
630  inextPt.y = cvFloor(nextPt.y);
631 
632  if( inextPt.x < -winSize.width || inextPt.x >= J.cols ||
633  inextPt.y < -winSize.height || inextPt.y >= J.rows )
634  {
635  if( level == 0 && status )
636  status[ptidx] = false;
637  break;
638  }
639 
640  a = nextPt.x - inextPt.x;
641  b = nextPt.y - inextPt.y;
642  iw00 = cvRound((1.f - a)*(1.f - b)*(1 << W_BITS));
643  iw01 = cvRound(a*(1.f - b)*(1 << W_BITS));
644  iw10 = cvRound((1.f - a)*b*(1 << W_BITS));
645  iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
646  acctype ib1 = 0, ib2 = 0;
647  float b1, b2;
648 
649  for( y = 0; y < winSize.height; y++ )
650  {
651  const uchar* Jptr = J.ptr() + (y + inextPt.y)*stepJ + inextPt.x*cn;
652  const short* Iptr = IWinBuf.ptr<short>(y);
653  const short* dIptr = derivIWinBuf.ptr<short>(y);
654 
655  x = 0;
656 
657  for( ; x < winSize.width*cn; x++, dIptr += 2 )
658  {
659  int diff = CV_DESCALE(Jptr[x]*iw00 + Jptr[x+cn]*iw01 +
660  Jptr[x+stepJ]*iw10 + Jptr[x+stepJ+cn]*iw11,
661  W_BITS1-5) - Iptr[x];
662  ib1 += (itemtype)(diff*dIptr[0]);
663  ib2 += (itemtype)(diff*dIptr[1]);
664  }
665  }
666 
667  b1 = ib1*FLT_SCALE;
668  b2 = ib2*FLT_SCALE;
669 
670  cv::Point2f delta( (float)((A12*b2 - A22*b1) * D),
671  0);//(float)((A12*b1 - A11*b2) * D)); // MODIFICATION
672  //delta = -delta;
673 
674  nextPt += delta;
675  nextPts[ptidx] = nextPt + halfWin;
676 
677  if( delta.ddot(delta) <= criteria.epsilon )
678  break;
679 
680  if( j > 0 && std::abs(delta.x + prevDelta.x) < 0.01 &&
681  std::abs(delta.y + prevDelta.y) < 0.01 )
682  {
683  nextPts[ptidx] -= delta*0.5f;
684  break;
685  }
686  prevDelta = delta;
687  }
688 
689  if( status[ptidx] && err && level == 0 && (flags & cv::OPTFLOW_LK_GET_MIN_EIGENVALS) == 0 )
690  {
691  cv::Point2f nextPoint = nextPts[ptidx] - halfWin;
692  cv::Point inextPoint;
693 
694  inextPoint.x = cvFloor(nextPoint.x);
695  inextPoint.y = cvFloor(nextPoint.y);
696 
697  if( inextPoint.x < -winSize.width || inextPoint.x >= J.cols ||
698  inextPoint.y < -winSize.height || inextPoint.y >= J.rows )
699  {
700  if( status )
701  status[ptidx] = false;
702  continue;
703  }
704 
705  float aa = nextPoint.x - inextPoint.x;
706  float bb = nextPoint.y - inextPoint.y;
707  iw00 = cvRound((1.f - aa)*(1.f - bb)*(1 << W_BITS));
708  iw01 = cvRound(aa*(1.f - bb)*(1 << W_BITS));
709  iw10 = cvRound((1.f - aa)*bb*(1 << W_BITS));
710  iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
711  float errval = 0.f;
712 
713  for( y = 0; y < winSize.height; y++ )
714  {
715  const uchar* Jptr = J.ptr() + (y + inextPoint.y)*stepJ + inextPoint.x*cn;
716  const short* Iptr = IWinBuf.ptr<short>(y);
717 
718  for( x = 0; x < winSize.width*cn; x++ )
719  {
720  int diff = CV_DESCALE(Jptr[x]*iw00 + Jptr[x+cn]*iw01 +
721  Jptr[x+stepJ]*iw10 + Jptr[x+stepJ+cn]*iw11,
722  W_BITS1-5) - Iptr[x];
723  errval += std::abs((float)diff);
724  }
725  }
726  err[ptidx] = errval * 1.f/(32*winSize.width*cn*winSize.height);
727  }
728  }
729  }
730 
731  }
732 }
733 
734 cv::Mat disparityFromStereoImages(
735  const cv::Mat & leftImage,
736  const cv::Mat & rightImage,
737  const ParametersMap & parameters)
738 {
739  UASSERT(!leftImage.empty() && !rightImage.empty());
740  UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
741  UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1);
742 
743  cv::Mat leftMono;
744  if(leftImage.channels() == 3)
745  {
746  cv::cvtColor(leftImage, leftMono, CV_BGR2GRAY);
747  }
748  else
749  {
750  leftMono = leftImage;
751  }
752  cv::Mat disparity;
753 
754  StereoBM stereo(parameters);
755  return stereo.computeDisparity(leftMono, rightImage);
756 }
757 
758 cv::Mat depthFromDisparity(const cv::Mat & disparity,
759  float fx, float baseline,
760  int type)
761 {
762  UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
763  UASSERT(type == CV_32FC1 || type == CV_16UC1);
764  cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type);
765  int countOverMax = 0;
766  for (int i = 0; i < disparity.rows; i++)
767  {
768  for (int j = 0; j < disparity.cols; j++)
769  {
770  float disparity_value = disparity.type() == CV_16SC1?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
771  if (disparity_value > 0.0f)
772  {
773  // baseline * focal / disparity
774  float d = baseline * fx / disparity_value;
775  if(d>0)
776  {
777  if(depth.type() == CV_32FC1)
778  {
779  depth.at<float>(i,j) = d;
780  }
781  else
782  {
783  if(d*1000.0f <= (float)USHRT_MAX)
784  {
785  depth.at<unsigned short>(i,j) = (unsigned short)(d*1000.0f);
786  }
787  else
788  {
789  ++countOverMax;
790  }
791  }
792  }
793  }
794  }
795  }
796  if(countOverMax)
797  {
798  UWARN("Depth conversion error, %d depth values ignored because they are over the maximum depth allowed (65535 mm).", countOverMax);
799  }
800  return depth;
801 }
802 
803 cv::Mat depthFromStereoImages(
804  const cv::Mat & leftImage,
805  const cv::Mat & rightImage,
806  const std::vector<cv::Point2f> & leftCorners,
807  float fx,
808  float baseline,
809  int flowWinSize,
810  int flowMaxLevel,
811  int flowIterations,
812  double flowEps)
813 {
814  UASSERT(!leftImage.empty() && !rightImage.empty() &&
815  leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
816  leftImage.cols == rightImage.cols &&
817  leftImage.rows == rightImage.rows);
818  UASSERT(fx > 0.0f && baseline > 0.0f);
819 
820  // Find features in the new left image
821  std::vector<unsigned char> status;
822  std::vector<float> err;
823  std::vector<cv::Point2f> rightCorners;
824  UDEBUG("cv::calcOpticalFlowPyrLK() begin");
825  cv::calcOpticalFlowPyrLK(
826  leftImage,
827  rightImage,
828  leftCorners,
829  rightCorners,
830  status,
831  err,
832  cv::Size(flowWinSize, flowWinSize), flowMaxLevel,
833  cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations, flowEps),
834  cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
835  UDEBUG("cv::calcOpticalFlowPyrLK() end");
836 
837  return depthFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, fx, baseline);
838 }
839 
840 cv::Mat disparityFromStereoCorrespondences(
841  const cv::Size & disparitySize,
842  const std::vector<cv::Point2f> & leftCorners,
843  const std::vector<cv::Point2f> & rightCorners,
844  const std::vector<unsigned char> & mask)
845 {
846  UASSERT(leftCorners.size() == rightCorners.size());
847  UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
848  cv::Mat disparity = cv::Mat::zeros(disparitySize, CV_32FC1);
849  for(unsigned int i=0; i<leftCorners.size(); ++i)
850  {
851  if(mask.empty() || mask[i])
852  {
853  cv::Point2i dispPt(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f));
854  UASSERT(dispPt.x >= 0 && dispPt.x < disparitySize.width);
855  UASSERT(dispPt.y >= 0 && dispPt.y < disparitySize.height);
856  disparity.at<float>(dispPt.y, dispPt.x) = leftCorners[i].x - rightCorners[i].x;
857  }
858  }
859  return disparity;
860 }
861 
862 cv::Mat depthFromStereoCorrespondences(
863  const cv::Mat & leftImage,
864  const std::vector<cv::Point2f> & leftCorners,
865  const std::vector<cv::Point2f> & rightCorners,
866  const std::vector<unsigned char> & mask,
867  float fx, float baseline)
868 {
869  UASSERT(!leftImage.empty() && leftCorners.size() == rightCorners.size());
870  UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
871  cv::Mat depth = cv::Mat::zeros(leftImage.rows, leftImage.cols, CV_32FC1);
872  for(unsigned int i=0; i<leftCorners.size(); ++i)
873  {
874  if(mask.size() == 0 || mask[i])
875  {
876  float disparity = leftCorners[i].x - rightCorners[i].x;
877  if(disparity > 0.0f)
878  {
879  float d = baseline * fx / disparity;
880  depth.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
881  }
882  }
883  }
884  return depth;
885 }
886 
887 cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
888 {
889  UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
890  cv::Mat depth16U;
891  if(!depth32F.empty())
892  {
893  depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
894  int countOverMax = 0;
895  for(int i=0; i<depth32F.rows; ++i)
896  {
897  for(int j=0; j<depth32F.cols; ++j)
898  {
899  float depth = (depth32F.at<float>(i,j)*1000.0f);
900  unsigned short depthMM = 0;
901  if(depth > 0 && depth <= (float)USHRT_MAX)
902  {
903  depthMM = (unsigned short)depth;
904  }
905  else if(depth > (float)USHRT_MAX)
906  {
907  ++countOverMax;
908  }
909  depth16U.at<unsigned short>(i, j) = depthMM;
910  }
911  }
912  if(countOverMax)
913  {
914  UWARN("Depth conversion error, %d depth values ignored because "
915  "they are over the maximum depth allowed (65535 mm). Is the depth "
916  "image really in meters? 32 bits images should be in meters, "
917  "and 16 bits should be in mm.", countOverMax);
918  }
919  }
920  return depth16U;
921 }
922 
923 cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
924 {
925  UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
926  cv::Mat depth32F;
927  if(!depth16U.empty())
928  {
929  depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
930  for(int i=0; i<depth16U.rows; ++i)
931  {
932  for(int j=0; j<depth16U.cols; ++j)
933  {
934  float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
935  depth32F.at<float>(i, j) = depth;
936  }
937  }
938  }
939  return depth32F;
940 }
941 
942 float getDepth(
943  const cv::Mat & depthImage,
944  float x, float y,
945  bool smoothing,
946  float depthErrorRatio,
947  bool estWithNeighborsIfNull)
948 {
949  UASSERT(!depthImage.empty());
950  UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
951 
952  int u = int(x+0.5f);
953  int v = int(y+0.5f);
954  if(u == depthImage.cols && x<float(depthImage.cols))
955  {
956  u = depthImage.cols - 1;
957  }
958  if(v == depthImage.rows && y<float(depthImage.rows))
959  {
960  v = depthImage.rows - 1;
961  }
962 
963  if(!(u >=0 && u<depthImage.cols && v >=0 && v<depthImage.rows))
964  {
965  UDEBUG("!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows) cond failed! returning bad point. (x=%f (u=%d), y=%f (v=%d), cols=%d, rows=%d)",
966  x,u,y,v,depthImage.cols, depthImage.rows);
967  return 0;
968  }
969 
970  bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
971 
972  // Inspired from RGBDFrame::getGaussianMixtureDistribution() method from
973  // https://github.com/ccny-ros-pkg/rgbdtools/blob/master/src/rgbd_frame.cpp
974  // Window weights:
975  // | 1 | 2 | 1 |
976  // | 2 | 4 | 2 |
977  // | 1 | 2 | 1 |
978  int u_start = std::max(u-1, 0);
979  int v_start = std::max(v-1, 0);
980  int u_end = std::min(u+1, depthImage.cols-1);
981  int v_end = std::min(v+1, depthImage.rows-1);
982 
983  float depth = 0.0f;
984  if(isInMM)
985  {
986  if(depthImage.at<unsigned short>(v,u) > 0 &&
987  depthImage.at<unsigned short>(v,u) < std::numeric_limits<unsigned short>::max())
988  {
989  depth = float(depthImage.at<unsigned short>(v,u))*0.001f;
990  }
991  }
992  else
993  {
994  depth = depthImage.at<float>(v,u);
995  }
996 
997  if((depth==0.0f || !uIsFinite(depth)) && estWithNeighborsIfNull)
998  {
999  // all cells no2 must be under the zError to be accepted
1000  float tmp = 0.0f;
1001  int count = 0;
1002  for(int uu = u_start; uu <= u_end; ++uu)
1003  {
1004  for(int vv = v_start; vv <= v_end; ++vv)
1005  {
1006  if((uu == u && vv!=v) || (uu != u && vv==v))
1007  {
1008  float d = 0.0f;
1009  if(isInMM)
1010  {
1011  if(depthImage.at<unsigned short>(vv,uu) > 0 &&
1012  depthImage.at<unsigned short>(vv,uu) < std::numeric_limits<unsigned short>::max())
1013  {
1014  d = float(depthImage.at<unsigned short>(vv,uu))*0.001f;
1015  }
1016  }
1017  else
1018  {
1019  d = depthImage.at<float>(vv,uu);
1020  }
1021  if(d!=0.0f && uIsFinite(d))
1022  {
1023  if(tmp == 0.0f)
1024  {
1025  tmp = d;
1026  ++count;
1027  }
1028  else
1029  {
1030  float depthError = depthErrorRatio * tmp;
1031  if(fabs(d - tmp/float(count)) < depthError)
1032 
1033  {
1034  tmp += d;
1035  ++count;
1036  }
1037  }
1038  }
1039  }
1040  }
1041  }
1042  if(count > 1)
1043  {
1044  depth = tmp/float(count);
1045  }
1046  }
1047 
1048  if(depth!=0.0f && uIsFinite(depth))
1049  {
1050  if(smoothing)
1051  {
1052  float sumWeights = 0.0f;
1053  float sumDepths = 0.0f;
1054  for(int uu = u_start; uu <= u_end; ++uu)
1055  {
1056  for(int vv = v_start; vv <= v_end; ++vv)
1057  {
1058  if(!(uu == u && vv == v))
1059  {
1060  float d = 0.0f;
1061  if(isInMM)
1062  {
1063  if(depthImage.at<unsigned short>(vv,uu) > 0 &&
1064  depthImage.at<unsigned short>(vv,uu) < std::numeric_limits<unsigned short>::max())
1065  {
1066  d = float(depthImage.at<unsigned short>(vv,uu))*0.001f;
1067  }
1068  }
1069  else
1070  {
1071  d = depthImage.at<float>(vv,uu);
1072  }
1073 
1074  float depthError = depthErrorRatio * depth;
1075 
1076  // ignore if not valid or depth difference is too high
1077  if(d != 0.0f && uIsFinite(d) && fabs(d - depth) < depthError)
1078  {
1079  if(uu == u || vv == v)
1080  {
1081  sumWeights+=2.0f;
1082  d*=2.0f;
1083  }
1084  else
1085  {
1086  sumWeights+=1.0f;
1087  }
1088  sumDepths += d;
1089  }
1090  }
1091  }
1092  }
1093  // set window weight to center point
1094  depth *= 4.0f;
1095  sumWeights += 4.0f;
1096 
1097  // mean
1098  depth = (depth+sumDepths)/sumWeights;
1099  }
1100  }
1101  else
1102  {
1103  depth = 0;
1104  }
1105  return depth;
1106 }
1107 
1108 cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios)
1109 {
1110  return computeRoi(image.size(), roiRatios);
1111 }
1112 
1113 cv::Rect computeRoi(const cv::Size & imageSize, const std::string & roiRatios)
1114 {
1115  std::list<std::string> strValues = uSplit(roiRatios, ' ');
1116  if(strValues.size() != 4)
1117  {
1118  UERROR("The number of values must be 4 (roi=\"%s\")", roiRatios.c_str());
1119  }
1120  else
1121  {
1122  std::vector<float> values(4);
1123  unsigned int i=0;
1124  for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
1125  {
1126  values[i] = uStr2Float(*iter);
1127  ++i;
1128  }
1129 
1130  if(values[0] >= 0 && values[0] < 1 && values[0] < 1.0f-values[1] &&
1131  values[1] >= 0 && values[1] < 1 && values[1] < 1.0f-values[0] &&
1132  values[2] >= 0 && values[2] < 1 && values[2] < 1.0f-values[3] &&
1133  values[3] >= 0 && values[3] < 1 && values[3] < 1.0f-values[2])
1134  {
1135  return computeRoi(imageSize, values);
1136  }
1137  else
1138  {
1139  UERROR("The roi ratios are not valid (roi=\"%s\")", roiRatios.c_str());
1140  }
1141  }
1142  return cv::Rect();
1143 }
1144 
1145 cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
1146 {
1147  return computeRoi(image.size(), roiRatios);
1148 }
1149 
1150 cv::Rect computeRoi(const cv::Size & imageSize, const std::vector<float> & roiRatios)
1151 {
1152  if(imageSize.height!=0 && imageSize.width!= 0 && roiRatios.size() == 4)
1153  {
1154  float width = imageSize.width;
1155  float height = imageSize.height;
1156  cv::Rect roi(0, 0, width, height);
1157  UDEBUG("roi ratios = %f, %f, %f, %f", roiRatios[0],roiRatios[1],roiRatios[2],roiRatios[3]);
1158  UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
1159 
1160  //left roi
1161  if(roiRatios[0] > 0 && roiRatios[0] < 1.0f - roiRatios[1])
1162  {
1163  roi.x = width * roiRatios[0];
1164  }
1165 
1166  //right roi
1167  if(roiRatios[1] > 0 && roiRatios[1] < 1.0f - roiRatios[0])
1168  {
1169  roi.width -= width * roiRatios[1];
1170  }
1171  roi.width -= roi.x;
1172 
1173  //top roi
1174  if(roiRatios[2] > 0 && roiRatios[2] < 1.0f - roiRatios[3])
1175  {
1176  roi.y = height * roiRatios[2];
1177  }
1178 
1179  //bottom roi
1180  if(roiRatios[3] > 0 && roiRatios[3] < 1.0f - roiRatios[2])
1181  {
1182  roi.height -= height * roiRatios[3];
1183  }
1184  roi.height -= roi.y;
1185  UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
1186 
1187  return roi;
1188  }
1189  else
1190  {
1191  UERROR("Image is null or _roiRatios(=%d) != 4", roiRatios.size());
1192  return cv::Rect();
1193  }
1194 }
1195 
1196 cv::Mat decimate(const cv::Mat & image, int decimation)
1197 {
1198  UASSERT(decimation >= 1);
1199  cv::Mat out;
1200  if(!image.empty())
1201  {
1202  if(decimation > 1)
1203  {
1204  if((image.type() == CV_32FC1 || image.type()==CV_16UC1))
1205  {
1206  UASSERT_MSG(image.rows % decimation == 0 && image.cols % decimation == 0,
1207  uFormat("Decimation of depth images should be exact! (decimation=%d, size=%dx%d)",
1208  decimation, image.cols, image.rows).c_str());
1209 
1210  out = cv::Mat(image.rows/decimation, image.cols/decimation, image.type());
1211  if(image.type() == CV_32FC1)
1212  {
1213  for(int j=0; j<out.rows; ++j)
1214  {
1215  for(int i=0; i<out.cols; ++i)
1216  {
1217  out.at<float>(j, i) = image.at<float>(j*decimation, i*decimation);
1218  }
1219  }
1220  }
1221  else // CV_16UC1
1222  {
1223  for(int j=0; j<out.rows; ++j)
1224  {
1225  for(int i=0; i<out.cols; ++i)
1226  {
1227  out.at<unsigned short>(j, i) = image.at<unsigned short>(j*decimation, i*decimation);
1228  }
1229  }
1230  }
1231  }
1232  else
1233  {
1234  cv::resize(image, out, cv::Size(), 1.0f/float(decimation), 1.0f/float(decimation), cv::INTER_AREA);
1235  }
1236  }
1237  else
1238  {
1239  out = image;
1240  }
1241  }
1242  return out;
1243 }
1244 
1245 cv::Mat interpolate(const cv::Mat & image, int factor, float depthErrorRatio)
1246 {
1247  UASSERT_MSG(factor >= 1, uFormat("factor=%d", factor).c_str());
1248  cv::Mat out;
1249  if(!image.empty())
1250  {
1251  if(factor > 1)
1252  {
1253  if((image.type() == CV_32FC1 || image.type()==CV_16UC1))
1254  {
1255  UASSERT(depthErrorRatio>0.0f);
1256  out = cv::Mat::zeros(image.rows*factor, image.cols*factor, image.type());
1257  for(int j=0; j<out.rows; j+=factor)
1258  {
1259  for(int i=0; i<out.cols; i+=factor)
1260  {
1261  if(i>0 && j>0)
1262  {
1263  float dTopLeft;
1264  float dTopRight;
1265  float dBottomLeft;
1266  float dBottomRight;
1267  if(image.type() == CV_32FC1)
1268  {
1269  dTopLeft = image.at<float>(j/factor-1, i/factor-1);
1270  dTopRight = image.at<float>(j/factor-1, i/factor);
1271  dBottomLeft = image.at<float>(j/factor, i/factor-1);
1272  dBottomRight = image.at<float>(j/factor, i/factor);
1273  }
1274  else
1275  {
1276  dTopLeft = image.at<unsigned short>(j/factor-1, i/factor-1);
1277  dTopRight = image.at<unsigned short>(j/factor-1, i/factor);
1278  dBottomLeft = image.at<unsigned short>(j/factor, i/factor-1);
1279  dBottomRight = image.at<unsigned short>(j/factor, i/factor);
1280  }
1281 
1282  if(dTopLeft>0 && dTopRight>0 && dBottomLeft>0 && dBottomRight > 0)
1283  {
1284  float depthError = depthErrorRatio*(dTopLeft+dTopRight+dBottomLeft+dBottomRight)/4.0f;
1285  if(fabs(dTopLeft-dTopRight) <= depthError &&
1286  fabs(dTopLeft-dBottomLeft) <= depthError &&
1287  fabs(dTopLeft-dBottomRight) <= depthError)
1288  {
1289  // bilinear interpolation
1290  // do first and last rows then columns
1291  float slopeTop = (dTopRight-dTopLeft)/float(factor);
1292  float slopeBottom = (dBottomRight-dBottomLeft)/float(factor);
1293  if(image.type() == CV_32FC1)
1294  {
1295  for(int z=i-factor; z<=i; ++z)
1296  {
1297  out.at<float>(j-factor, z) = dTopLeft+(slopeTop*float(z-(i-factor)));
1298  out.at<float>(j, z) = dBottomLeft+(slopeBottom*float(z-(i-factor)));
1299  }
1300  }
1301  else
1302  {
1303  for(int z=i-factor; z<=i; ++z)
1304  {
1305  out.at<unsigned short>(j-factor, z) = (unsigned short)(dTopLeft+(slopeTop*float(z-(i-factor))));
1306  out.at<unsigned short>(j, z) = (unsigned short)(dBottomLeft+(slopeBottom*float(z-(i-factor))));
1307  }
1308  }
1309 
1310  // fill the columns
1311  if(image.type() == CV_32FC1)
1312  {
1313  for(int z=i-factor; z<=i; ++z)
1314  {
1315  float top = out.at<float>(j-factor, z);
1316  float bottom = out.at<float>(j, z);
1317  float slope = (bottom-top)/float(factor);
1318  for(int d=j-factor+1; d<j; ++d)
1319  {
1320  out.at<float>(d, z) = top+(slope*float(d-(j-factor)));
1321  }
1322  }
1323  }
1324  else
1325  {
1326  for(int z=i-factor; z<=i; ++z)
1327  {
1328  float top = out.at<unsigned short>(j-factor, z);
1329  float bottom = out.at<unsigned short>(j, z);
1330  float slope = (bottom-top)/float(factor);
1331  for(int d=j-factor+1; d<j; ++d)
1332  {
1333  out.at<unsigned short>(d, z) = (unsigned short)(top+(slope*float(d-(j-factor))));
1334  }
1335  }
1336  }
1337  }
1338  }
1339  }
1340  }
1341  }
1342  }
1343  else
1344  {
1345  cv::resize(image, out, cv::Size(), float(factor), float(factor));
1346  }
1347  }
1348  else
1349  {
1350  out = image;
1351  }
1352  }
1353  return out;
1354 }
1355 
1356 // Registration Depth to RGB (return registered depth image)
1357 cv::Mat registerDepth(
1358  const cv::Mat & depth,
1359  const cv::Mat & depthK,
1360  const cv::Size & colorSize,
1361  const cv::Mat & colorK,
1362  const rtabmap::Transform & transform)
1363 {
1364  UASSERT(!transform.isNull());
1365  UASSERT(!depth.empty());
1366  UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1); // mm or m
1367  UASSERT(depthK.type() == CV_64FC1 && depthK.cols == 3 && depthK.cols == 3);
1368  UASSERT(colorK.type() == CV_64FC1 && colorK.cols == 3 && colorK.cols == 3);
1369 
1370  float fx = depthK.at<double>(0,0);
1371  float fy = depthK.at<double>(1,1);
1372  float cx = depthK.at<double>(0,2);
1373  float cy = depthK.at<double>(1,2);
1374 
1375  float rfx = colorK.at<double>(0,0);
1376  float rfy = colorK.at<double>(1,1);
1377  float rcx = colorK.at<double>(0,2);
1378  float rcy = colorK.at<double>(1,2);
1379 
1380  //UDEBUG("depth(%dx%d) fx=%f fy=%f cx=%f cy=%f", depth.cols, depth.rows, fx, fy, cx, cy);
1381  //UDEBUG("color(%dx%d) fx=%f fy=%f cx=%f cy=%f", colorSize.width, colorSize.height, rfx, rfy, rcx, rcy);
1382 
1383  Eigen::Affine3f proj = transform.toEigen3f();
1384  Eigen::Vector4f P4,P3;
1385  P4[3] = 1;
1386  cv::Mat registered = cv::Mat::zeros(colorSize, depth.type());
1387 
1388  bool depthInMM = depth.type() == CV_16UC1;
1389  for(int y=0; y<depth.rows; ++y)
1390  {
1391  for(int x=0; x<depth.cols; ++x)
1392  {
1393  //filtering
1394  float dz = depthInMM?float(depth.at<unsigned short>(y,x))*0.001f:depth.at<float>(y,x); // put in meter for projection
1395  if(dz>=0.0f)
1396  {
1397  // Project to 3D
1398  P4[0] = (x - cx) * dz / fx; // Optimization: we could have (x-cx)/fx in a lookup table
1399  P4[1] = (y - cy) * dz / fy; // Optimization: we could have (y-cy)/fy in a lookup table
1400  P4[2] = dz;
1401 
1402  P3 = proj * P4;
1403  float z = P3[2];
1404  float invZ = 1.0f/z;
1405  int dx = (rfx*P3[0])*invZ + rcx;
1406  int dy = (rfy*P3[1])*invZ + rcy;
1407 
1408  if(uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
1409  {
1410  if(depthInMM)
1411  {
1412  unsigned short z16 = z * 1000; //mm
1413  unsigned short &zReg = registered.at<unsigned short>(dy, dx);
1414  if(zReg == 0 || z16 < zReg)
1415  {
1416  zReg = z16;
1417  }
1418  }
1419  else
1420  {
1421  float &zReg = registered.at<float>(dy, dx);
1422  if(zReg == 0 || z < zReg)
1423  {
1424  zReg = z;
1425  }
1426  }
1427  }
1428  }
1429  }
1430  }
1431  return registered;
1432 }
1433 
1434 cv::Mat fillDepthHoles(const cv::Mat & depth, int maximumHoleSize, float errorRatio)
1435 {
1436  UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
1437  UASSERT(maximumHoleSize > 0);
1438  cv::Mat output = depth.clone();
1439  bool isMM = depth.type() == CV_16UC1;
1440  for(int y=0; y<depth.rows-2; ++y)
1441  {
1442  for(int x=0; x<depth.cols-2; ++x)
1443  {
1444  float a, bRight, bDown;
1445  if(isMM)
1446  {
1447  a = depth.at<unsigned short>(y, x);
1448  bRight = depth.at<unsigned short>(y, x+1);
1449  bDown = depth.at<unsigned short>(y+1, x);
1450  }
1451  else
1452  {
1453  a = depth.at<float>(y, x);
1454  bRight = depth.at<float>(y, x+1);
1455  bDown = depth.at<float>(y+1, x);
1456  }
1457 
1458  if(a > 0.0f && (bRight == 0.0f || bDown == 0.0f))
1459  {
1460  bool horizontalSet = bRight != 0.0f;
1461  bool verticalSet = bDown != 0.0f;
1462  int stepX = 0;
1463  for(int h=1; h<=maximumHoleSize && (!horizontalSet || !verticalSet); ++h)
1464  {
1465  // horizontal
1466  if(!horizontalSet)
1467  {
1468  if(x+1+h >= depth.cols)
1469  {
1470  horizontalSet = true;
1471  }
1472  else
1473  {
1474  float c = isMM?depth.at<unsigned short>(y, x+1+h):depth.at<float>(y, x+1+h);
1475  if(c == 0)
1476  {
1477  // ignore this size
1478  }
1479  else
1480  {
1481  // fill hole
1482  float depthError = errorRatio*float(a+c)/2.0f;
1483  if(fabs(a-c) <= depthError)
1484  {
1485  //linear interpolation
1486  float slope = (c-a)/float(h+1);
1487  if(isMM)
1488  {
1489  for(int z=x+1; z<x+1+h; ++z)
1490  {
1491  unsigned short & value = output.at<unsigned short>(y, z);
1492  if(value == 0)
1493  {
1494  value = (unsigned short)(a+(slope*float(z-x)));
1495  }
1496  else
1497  {
1498  // average with the previously set value
1499  value = (value+(unsigned short)(a+(slope*float(z-x))))/2;
1500  }
1501  }
1502  }
1503  else
1504  {
1505  for(int z=x+1; z<x+1+h; ++z)
1506  {
1507  float & value = output.at<float>(y, z);
1508  if(value == 0)
1509  {
1510  value = a+(slope*float(z-x));
1511  }
1512  else
1513  {
1514  // average with the previously set value
1515  value = (value+(a+(slope*float(z-x))))/2;
1516  }
1517  }
1518  }
1519  }
1520  horizontalSet = true;
1521  stepX = h;
1522  }
1523  }
1524  }
1525 
1526  // vertical
1527  if(!verticalSet)
1528  {
1529  if(y+1+h >= depth.rows)
1530  {
1531  verticalSet = true;
1532  }
1533  else
1534  {
1535  float c = isMM?depth.at<unsigned short>(y+1+h, x):depth.at<float>(y+1+h, x);
1536  if(c == 0)
1537  {
1538  // ignore this size
1539  }
1540  else
1541  {
1542  // fill hole
1543  float depthError = errorRatio*float(a+c)/2.0f;
1544  if(fabs(a-c) <= depthError)
1545  {
1546  //linear interpolation
1547  float slope = (c-a)/float(h+1);
1548  if(isMM)
1549  {
1550  for(int z=y+1; z<y+1+h; ++z)
1551  {
1552  unsigned short & value = output.at<unsigned short>(z, x);
1553  if(value == 0)
1554  {
1555  value = (unsigned short)(a+(slope*float(z-y)));
1556  }
1557  else
1558  {
1559  // average with the previously set value
1560  value = (value+(unsigned short)(a+(slope*float(z-y))))/2;
1561  }
1562  }
1563  }
1564  else
1565  {
1566  for(int z=y+1; z<y+1+h; ++z)
1567  {
1568  float & value = output.at<float>(z, x);
1569  if(value == 0)
1570  {
1571  value = (a+(slope*float(z-y)));
1572  }
1573  else
1574  {
1575  // average with the previously set value
1576  value = (value+(a+(slope*float(z-y))))/2;
1577  }
1578  }
1579  }
1580  }
1581  verticalSet = true;
1582  }
1583  }
1584  }
1585  }
1586  x+=stepX;
1587  }
1588  }
1589  }
1590  return output;
1591 }
1592 
1593 void fillRegisteredDepthHoles(cv::Mat & registeredDepth, bool vertical, bool horizontal, bool fillDoubleHoles)
1594 {
1595  UASSERT(registeredDepth.type() == CV_16UC1);
1596  int margin = fillDoubleHoles?2:1;
1597  for(int x=1; x<registeredDepth.cols-margin; ++x)
1598  {
1599  for(int y=1; y<registeredDepth.rows-margin; ++y)
1600  {
1601  unsigned short & b = registeredDepth.at<unsigned short>(y, x);
1602  bool set = false;
1603  if(vertical)
1604  {
1605  const unsigned short & a = registeredDepth.at<unsigned short>(y-1, x);
1606  unsigned short & c = registeredDepth.at<unsigned short>(y+1, x);
1607  if(a && c)
1608  {
1609  unsigned short error = 0.01*((a+c)/2);
1610  if(((b == 0 && a && c) || (b > a+error && b > c+error)) &&
1611  (a>c?a-c<=error:c-a<=error))
1612  {
1613  b = (a+c)/2;
1614  set = true;
1615  if(!horizontal)
1616  {
1617  ++y;
1618  }
1619  }
1620  }
1621  if(!set && fillDoubleHoles)
1622  {
1623  const unsigned short & d = registeredDepth.at<unsigned short>(y+2, x);
1624  if(a && d && (b==0 || c==0))
1625  {
1626  unsigned short error = 0.01*((a+d)/2);
1627  if(((b == 0 && a && d) || (b > a+error && b > d+error)) &&
1628  ((c == 0 && a && d) || (c > a+error && c > d+error)) &&
1629  (a>d?a-d<=error:d-a<=error))
1630  {
1631  if(a>d)
1632  {
1633  unsigned short tmp = (a-d)/4;
1634  b = d + tmp;
1635  c = d + 3*tmp;
1636  }
1637  else
1638  {
1639  unsigned short tmp = (d-a)/4;
1640  b = a + tmp;
1641  c = a + 3*tmp;
1642  }
1643  set = true;
1644  if(!horizontal)
1645  {
1646  y+=2;
1647  }
1648  }
1649  }
1650  }
1651  }
1652  if(!set && horizontal)
1653  {
1654  const unsigned short & a = registeredDepth.at<unsigned short>(y, x-1);
1655  unsigned short & c = registeredDepth.at<unsigned short>(y, x+1);
1656  if(a && c)
1657  {
1658  unsigned short error = 0.01*((a+c)/2);
1659  if(((b == 0 && a && c) || (b > a+error && b > c+error)) &&
1660  (a>c?a-c<=error:c-a<=error))
1661  {
1662  b = (a+c)/2;
1663  set = true;
1664  }
1665  }
1666  if(!set && fillDoubleHoles)
1667  {
1668  const unsigned short & d = registeredDepth.at<unsigned short>(y, x+2);
1669  if(a && d && (b==0 || c==0))
1670  {
1671  unsigned short error = 0.01*((a+d)/2);
1672  if(((b == 0 && a && d) || (b > a+error && b > d+error)) &&
1673  ((c == 0 && a && d) || (c > a+error && c > d+error)) &&
1674  (a>d?a-d<=error:d-a<=error))
1675  {
1676  if(a>d)
1677  {
1678  unsigned short tmp = (a-d)/4;
1679  b = d + tmp;
1680  c = d + 3*tmp;
1681  }
1682  else
1683  {
1684  unsigned short tmp = (d-a)/4;
1685  b = a + tmp;
1686  c = a + 3*tmp;
1687  }
1688  }
1689  }
1690  }
1691  }
1692  }
1693  }
1694 }
1695 
1696 // used only for fastBilateralFiltering() below
1697 class Array3D
1698  {
1699  public:
1700  Array3D (const size_t width, const size_t height, const size_t depth)
1701  {
1702  x_dim_ = width;
1703  y_dim_ = height;
1704  z_dim_ = depth;
1705  v_ = std::vector<Eigen::Vector2f> (width*height*depth, Eigen::Vector2f (0.0f, 0.0f));
1706  }
1707 
1708  inline Eigen::Vector2f&
1709  operator () (const size_t x, const size_t y, const size_t z)
1710  { return v_[(x * y_dim_ + y) * z_dim_ + z]; }
1711 
1712  inline const Eigen::Vector2f&
1713  operator () (const size_t x, const size_t y, const size_t z) const
1714  { return v_[(x * y_dim_ + y) * z_dim_ + z]; }
1715 
1716  inline void
1717  resize (const size_t width, const size_t height, const size_t depth)
1718  {
1719  x_dim_ = width;
1720  y_dim_ = height;
1721  z_dim_ = depth;
1722  v_.resize (x_dim_ * y_dim_ * z_dim_);
1723  }
1724 
1725  Eigen::Vector2f
1726  trilinear_interpolation (const float x,
1727  const float y,
1728  const float z)
1729  {
1730  const size_t x_index = clamp (0, x_dim_ - 1, static_cast<size_t> (x));
1731  const size_t xx_index = clamp (0, x_dim_ - 1, x_index + 1);
1732 
1733  const size_t y_index = clamp (0, y_dim_ - 1, static_cast<size_t> (y));
1734  const size_t yy_index = clamp (0, y_dim_ - 1, y_index + 1);
1735 
1736  const size_t z_index = clamp (0, z_dim_ - 1, static_cast<size_t> (z));
1737  const size_t zz_index = clamp (0, z_dim_ - 1, z_index + 1);
1738 
1739  const float x_alpha = x - static_cast<float> (x_index);
1740  const float y_alpha = y - static_cast<float> (y_index);
1741  const float z_alpha = z - static_cast<float> (z_index);
1742 
1743  return
1744  (1.0f-x_alpha) * (1.0f-y_alpha) * (1.0f-z_alpha) * (*this)(x_index, y_index, z_index) +
1745  x_alpha * (1.0f-y_alpha) * (1.0f-z_alpha) * (*this)(xx_index, y_index, z_index) +
1746  (1.0f-x_alpha) * y_alpha * (1.0f-z_alpha) * (*this)(x_index, yy_index, z_index) +
1747  x_alpha * y_alpha * (1.0f-z_alpha) * (*this)(xx_index, yy_index, z_index) +
1748  (1.0f-x_alpha) * (1.0f-y_alpha) * z_alpha * (*this)(x_index, y_index, zz_index) +
1749  x_alpha * (1.0f-y_alpha) * z_alpha * (*this)(xx_index, y_index, zz_index) +
1750  (1.0f-x_alpha) * y_alpha * z_alpha * (*this)(x_index, yy_index, zz_index) +
1751  x_alpha * y_alpha * z_alpha * (*this)(xx_index, yy_index, zz_index);
1752  }
1753 
1754  static inline size_t
1755  clamp (const size_t min_value,
1756  const size_t max_value,
1757  const size_t x)
1758  {
1759  if (x >= min_value && x <= max_value)
1760  {
1761  return x;
1762  }
1763  else if (x < min_value)
1764  {
1765  return (min_value);
1766  }
1767  else
1768  {
1769  return (max_value);
1770  }
1771  }
1772 
1773  inline size_t
1774  x_size () const
1775  { return x_dim_; }
1776 
1777  inline size_t
1778  y_size () const
1779  { return y_dim_; }
1780 
1781  inline size_t
1782  z_size () const
1783  { return z_dim_; }
1784 
1785  inline std::vector<Eigen::Vector2f >::iterator
1786  begin ()
1787  { return v_.begin (); }
1788 
1789  inline std::vector<Eigen::Vector2f >::iterator
1790  end ()
1791  { return v_.end (); }
1792 
1793  inline std::vector<Eigen::Vector2f >::const_iterator
1794  begin () const
1795  { return v_.begin (); }
1796 
1797  inline std::vector<Eigen::Vector2f >::const_iterator
1798  end () const
1799  { return v_.end (); }
1800 
1801  private:
1802  std::vector<Eigen::Vector2f > v_;
1803  size_t x_dim_, y_dim_, z_dim_;
1804  };
1805 
1809 cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR, bool earlyDivision)
1810 {
1811  UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1));
1812  UDEBUG("Begin: depth float=%d %dx%d sigmaS=%f sigmaR=%f earlDivision=%d",
1813  depth.type()==CV_32FC1?1:0, depth.cols, depth.rows, sigmaS, sigmaR, earlyDivision?1:0);
1814 
1815  cv::Mat output = cv::Mat::zeros(depth.size(), CV_32FC1);
1816 
1817  float base_max = -std::numeric_limits<float>::max ();
1818  float base_min = std::numeric_limits<float>::max ();
1819  bool found_finite = false;
1820  for (int x = 0; x < depth.cols; ++x)
1821  for (int y = 0; y < depth.rows; ++y)
1822  {
1823  float z = depth.type()==CV_32FC1?depth.at<float>(y, x):float(depth.at<unsigned short>(y, x))/1000.0f;
1824  if (z > 0.0f && uIsFinite(z))
1825  {
1826  if (base_max < z)
1827  base_max = z;
1828  if (base_min > z)
1829  base_min = z;
1830  found_finite = true;
1831  }
1832  }
1833  if (!found_finite)
1834  {
1835  UWARN("Given an empty depth image. Doing nothing.");
1836  return cv::Mat();
1837  }
1838  UDEBUG("base_min=%f base_max=%f", base_min, base_max);
1839 
1840  const float base_delta = base_max - base_min;
1841 
1842  const size_t padding_xy = 2;
1843  const size_t padding_z = 2;
1844 
1845  const size_t small_width = static_cast<size_t> (static_cast<float> (depth.cols - 1) / sigmaS) + 1 + 2 * padding_xy;
1846  const size_t small_height = static_cast<size_t> (static_cast<float> (depth.rows - 1) / sigmaS) + 1 + 2 * padding_xy;
1847  const size_t small_depth = static_cast<size_t> (base_delta / sigmaR) + 1 + 2 * padding_z;
1848 
1849  UDEBUG("small_width=%d small_height=%d small_depth=%d", (int)small_width, (int)small_height, (int)small_depth);
1850  Array3D data (small_width, small_height, small_depth);
1851  for (int x = 0; x < depth.cols; ++x)
1852  {
1853  const size_t small_x = static_cast<size_t> (static_cast<float> (x) / sigmaS + 0.5f) + padding_xy;
1854  for (int y = 0; y < depth.rows; ++y)
1855  {
1856  float v = depth.type()==CV_32FC1?depth.at<float>(y,x):float(depth.at<unsigned short>(y,x))/1000.0f;
1857  if((v > 0 && uIsFinite(v)))
1858  {
1859  float z = v - base_min;
1860 
1861  const size_t small_y = static_cast<size_t> (static_cast<float> (y) / sigmaS + 0.5f) + padding_xy;
1862  const size_t small_z = static_cast<size_t> (static_cast<float> (z) / sigmaR + 0.5f) + padding_z;
1863 
1864  Eigen::Vector2f& d = data (small_x, small_y, small_z);
1865  d[0] += v;
1866  d[1] += 1.0f;
1867  }
1868  }
1869  }
1870 
1871  std::vector<long int> offset (3);
1872  offset[0] = &(data (1,0,0)) - &(data (0,0,0));
1873  offset[1] = &(data (0,1,0)) - &(data (0,0,0));
1874  offset[2] = &(data (0,0,1)) - &(data (0,0,0));
1875 
1876  Array3D buffer (small_width, small_height, small_depth);
1877 
1878  for (size_t dim = 0; dim < 3; ++dim)
1879  {
1880  const long int off = offset[dim];
1881  for (size_t n_iter = 0; n_iter < 2; ++n_iter)
1882  {
1883  std::swap (buffer, data);
1884  for(size_t x = 1; x < small_width - 1; ++x)
1885  for(size_t y = 1; y < small_height - 1; ++y)
1886  {
1887  Eigen::Vector2f* d_ptr = &(data (x,y,1));
1888  Eigen::Vector2f* b_ptr = &(buffer (x,y,1));
1889 
1890  for(size_t z = 1; z < small_depth - 1; ++z, ++d_ptr, ++b_ptr)
1891  *d_ptr = (*(b_ptr - off) + *(b_ptr + off) + 2.0 * (*b_ptr)) / 4.0;
1892  }
1893  }
1894  }
1895 
1896  if (earlyDivision)
1897  {
1898  for (std::vector<Eigen::Vector2f>::iterator d = data.begin (); d != data.end (); ++d)
1899  *d /= ((*d)[0] != 0) ? (*d)[1] : 1;
1900  }
1901 
1902  for (int x = 0; x < depth.cols; ++x)
1903  for (int y = 0; y < depth.rows; ++y)
1904  {
1905  float z = depth.type()==CV_32FC1?depth.at<float>(y,x):float(depth.at<unsigned short>(y,x))/1000.0f;
1906  if(z > 0 && uIsFinite(z))
1907  {
1908  z -= base_min;
1909  const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigmaS + padding_xy,
1910  static_cast<float> (y) / sigmaS + padding_xy,
1911  z / sigmaR + padding_z);
1912  float v = earlyDivision ? D[0] : D[0] / D[1];
1913  if(v < base_min || v >= base_max)
1914  {
1915  v = 0.0f;
1916  }
1917  if(depth.type()==CV_16UC1 && v>65.5350f)
1918  {
1919  v = 65.5350f;
1920  }
1921  output.at<float>(y,x) = v;
1922  }
1923  }
1924 
1925  UDEBUG("End");
1926  return output;
1927 }
1928 
1937 cv::Mat brightnessAndContrastAuto(const cv::Mat &src, const cv::Mat & mask, float clipLowHistPercent, float clipHighHistPercent)
1938 {
1939 
1940  CV_Assert(clipLowHistPercent >= 0 && clipHighHistPercent>=0);
1941  CV_Assert((src.type() == CV_8UC1) || (src.type() == CV_8UC3) || (src.type() == CV_8UC4));
1942 
1943  int histSize = 256;
1944  float alpha, beta;
1945  double minGray = 0, maxGray = 0;
1946 
1947  //to calculate grayscale histogram
1948  cv::Mat gray;
1949  if (src.type() == CV_8UC1) gray = src;
1950  else if (src.type() == CV_8UC3) cvtColor(src, gray, CV_BGR2GRAY);
1951  else if (src.type() == CV_8UC4) cvtColor(src, gray, CV_BGRA2GRAY);
1952  if (clipLowHistPercent == 0 && clipHighHistPercent == 0)
1953  {
1954  // keep full available range
1955  cv::minMaxLoc(gray, &minGray, &maxGray, 0, 0, mask);
1956  }
1957  else
1958  {
1959  cv::Mat hist; //the grayscale histogram
1960 
1961  float range[] = { 0, 256 };
1962  const float* histRange = { range };
1963  bool uniform = true;
1964  bool accumulate = false;
1965  calcHist(&gray, 1, 0, mask, hist, 1, &histSize, &histRange, uniform, accumulate);
1966 
1967  // calculate cumulative distribution from the histogram
1968  std::vector<float> accumulator(histSize);
1969  accumulator[0] = hist.at<float>(0);
1970  for (int i = 1; i < histSize; i++)
1971  {
1972  accumulator[i] = accumulator[i - 1] + hist.at<float>(i);
1973  }
1974 
1975  // locate points that cuts at required value
1976  float max = accumulator.back();
1977  clipLowHistPercent *= (max / 100.0); //make percent as absolute
1978  clipHighHistPercent *= (max / 100.0); //make percent as absolute
1979  // locate left cut
1980  minGray = 0;
1981  while (accumulator[minGray] < clipLowHistPercent)
1982  minGray++;
1983 
1984  // locate right cut
1985  maxGray = histSize - 1;
1986  while (accumulator[maxGray] >= (max - clipHighHistPercent))
1987  maxGray--;
1988  }
1989 
1990  // current range
1991  float inputRange = maxGray - minGray;
1992 
1993  alpha = (histSize - 1) / inputRange; // alpha expands current range to histsize range
1994  beta = -minGray * alpha; // beta shifts current range so that minGray will go to 0
1995 
1996  UINFO("minGray=%f maxGray=%f alpha=%f beta=%f", minGray, maxGray, alpha, beta);
1997 
1998  cv::Mat dst;
1999  // Apply brightness and contrast normalization
2000  // convertTo operates with saurate_cast
2001  src.convertTo(dst, -1, alpha, beta);
2002 
2003  // restore alpha channel from source
2004  if (dst.type() == CV_8UC4)
2005  {
2006  int from_to[] = { 3, 3};
2007  cv::mixChannels(&src, 4, &dst,1, from_to, 1);
2008  }
2009  return dst;
2010 }
2011 
2012 cv::Mat exposureFusion(const std::vector<cv::Mat> & images)
2013 {
2014  UASSERT(images.size());
2015  cv::Mat fusion;
2016 #if CV_MAJOR_VERSION >= 3
2017  cv::createMergeMertens()->process(images, fusion);
2018  cv::Mat rgb8;
2019  UASSERT(fusion.channels() == 3);
2020  fusion.convertTo(rgb8, CV_8UC3, 255.0);
2021  fusion = rgb8;
2022 #else
2023  UWARN("Exposure fusion is only available when rtabmap is built with OpenCV3.");
2024  if (images.size())
2025  {
2026  fusion = images[0].clone();
2027  }
2028 #endif
2029  return fusion;
2030 }
2031 
2032 }
2033 
2034 }
rtabmap::util2d::disparityFromStereoImages
cv::Mat RTABMAP_EXP disparityFromStereoImages(const cv::Mat &leftImage, const cv::Mat &rightImage, const ParametersMap &parameters=ParametersMap())
Definition: util2d.cpp:734
d
d
glm::mask
GLM_FUNC_DECL genIType mask(genIType const &count)
rtabmap::util2d::depthFromStereoCorrespondences
cv::Mat RTABMAP_EXP depthFromStereoCorrespondences(const cv::Mat &leftImage, const std::vector< cv::Point2f > &leftCorners, const std::vector< cv::Point2f > &rightCorners, const std::vector< unsigned char > &mask, float fx, float baseline)
Definition: util2d.cpp:862
rtabmap::util2d::Array3D::end
std::vector< Eigen::Vector2f >::const_iterator end() const
Definition: util2d.cpp:1798
UTimer
Definition: UTimer.h:46
rtabmap::util2d::Array3D::end
std::vector< Eigen::Vector2f >::iterator end()
Definition: util2d.cpp:1790
rtabmap::util2d::cvtDepthToFloat
cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat &depth16U)
Definition: util2d.cpp:923
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
CV_DESCALE
#define CV_DESCALE(x, n)
Definition: util2d.cpp:354
f
f
uIsInBounds
bool uIsInBounds(const T &value, const T &low, const T &high)
Definition: UMath.h:934
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::util2d::Array3D::trilinear_interpolation
Eigen::Vector2f trilinear_interpolation(const float x, const float y, const float z)
Definition: util2d.cpp:1726
rtflann::uchar
unsigned char uchar
Definition: matrix.h:41
rtabmap::util2d::depthFromDisparity
cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat &disparity, float fx, float baseline, int type=CV_32FC1)
Definition: util2d.cpp:758
glm::e
GLM_FUNC_DECL genType e()
uStr2Float
float UTILITE_EXP uStr2Float(const std::string &str)
Definition: UConversion.cpp:123
dst
char * dst
Definition: lz4.h:354
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:41
rtabmap::util2d::fastBilateralFiltering
cv::Mat RTABMAP_EXP fastBilateralFiltering(const cv::Mat &depth, float sigmaS=15.0f, float sigmaR=0.05f, bool earlyDivision=false)
Definition: util2d.cpp:1809
UMath.h
Basic mathematics functions.
UConversion.h
Some conversion functions.
rtabmap::util2d::registerDepth
cv::Mat RTABMAP_EXP registerDepth(const cv::Mat &depth, const cv::Mat &depthK, const cv::Size &colorSize, const cv::Mat &colorK, const rtabmap::Transform &transform)
Definition: util2d.cpp:1357
rtabmap::util2d::depthFromStereoImages
cv::Mat RTABMAP_EXP depthFromStereoImages(const cv::Mat &leftImage, const cv::Mat &rightImage, const std::vector< cv::Point2f > &leftCorners, float fx, float baseline, int flowWinSize=9, int flowMaxLevel=4, int flowIterations=20, double flowEps=0.02)
Definition: util2d.cpp:803
rtabmap::util2d::Array3D::v_
std::vector< Eigen::Vector2f > v_
Definition: util2d.cpp:1802
rtabmap::util2d::Array3D::begin
std::vector< Eigen::Vector2f >::const_iterator begin() const
Definition: util2d.cpp:1794
glm::step
GLM_FUNC_DECL genType step(genType const &edge, genType const &x)
uSplit
std::list< std::string > uSplit(const std::string &str, char separator= ' ')
Definition: UStl.h:566
rtabmap::util2d::interpolate
cv::Mat RTABMAP_EXP interpolate(const cv::Mat &image, int factor, float depthErrorRatio=0.02f)
Definition: util2d.cpp:1245
rtabmap::util2d::ssd
float RTABMAP_EXP ssd(const cv::Mat &windowLeft, const cv::Mat &windowRight)
Definition: util2d.cpp:56
rtabmap::util2d::Array3D::Array3D
Array3D(const size_t width, const size_t height, const size_t depth)
Definition: util2d.cpp:1700
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:55
UASSERT
#define UASSERT(condition)
UStl.h
Wrappers of STL for convenient functions.
rtabmap::StereoBM::computeDisparity
virtual cv::Mat computeDisparity(const cv::Mat &leftImage, const cv::Mat &rightImage) const
Definition: StereoDense.cpp:76
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
glm::floor
GLM_FUNC_DECL genType floor(genType const &x)
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
cvtColor
CvImagePtr cvtColor(const CvImageConstPtr &source, const std::string &encoding)
glm::proj
GLM_FUNC_DECL vecType proj(vecType const &x, vecType const &Normal)
rtabmap::util2d::decimate
cv::Mat RTABMAP_EXP decimate(const cv::Mat &image, int d)
Definition: util2d.cpp:1196
rtabmap::util2d::Array3D::z_size
size_t z_size() const
Definition: util2d.cpp:1782
rtabmap::util2d::brightnessAndContrastAuto
cv::Mat RTABMAP_EXP brightnessAndContrastAuto(const cv::Mat &src, const cv::Mat &mask, float clipLowHistPercent=0, float clipHighHistPercent=0)
Automatic brightness and contrast optimization with optional histogram clipping.
Definition: util2d.cpp:1937
rtabmap::util2d::disparityFromStereoCorrespondences
cv::Mat RTABMAP_EXP disparityFromStereoCorrespondences(const cv::Size &disparitySize, const std::vector< cv::Point2f > &leftCorners, const std::vector< cv::Point2f > &rightCorners, const std::vector< unsigned char > &mask)
Definition: util2d.cpp:840
glm::abs
GLM_FUNC_DECL genType abs(genType const &x)
rtabmap::util2d::computeRoi
cv::Rect RTABMAP_EXP computeRoi(const cv::Mat &image, const std::string &roiRatios)
Definition: util2d.cpp:1108
util3d_transforms.h
rtabmap::util2d::fillRegisteredDepthHoles
void RTABMAP_EXP fillRegisteredDepthHoles(cv::Mat &depthRegistered, bool vertical, bool horizontal, bool fillDoubleHoles=false)
Definition: util2d.cpp:1593
rtabmap::util2d::Array3D::begin
std::vector< Eigen::Vector2f >::iterator begin()
Definition: util2d.cpp:1786
rtabmap::util2d::exposureFusion
cv::Mat RTABMAP_EXP exposureFusion(const std::vector< cv::Mat > &images)
Definition: util2d.cpp:2012
rtabmap::util2d::Array3D::y_size
size_t y_size() const
Definition: util2d.cpp:1778
UDEBUG
#define UDEBUG(...)
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
UERROR
#define UERROR(...)
rtabmap::util2d::fillDepthHoles
cv::Mat RTABMAP_EXP fillDepthHoles(const cv::Mat &depth, int maximumHoleSize=1, float errorRatio=0.02f)
Definition: util2d.cpp:1434
rtabmap::util2d::Array3D::resize
void resize(const size_t width, const size_t height, const size_t depth)
Definition: util2d.cpp:1717
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::util2d::Array3D::clamp
static size_t clamp(const size_t min_value, const size_t max_value, const size_t x)
Definition: util2d.cpp:1755
UTimer::ticks
double ticks()
Definition: UTimer.cpp:110
rtabmap::util2d::sad
float RTABMAP_EXP sad(const cv::Mat &windowLeft, const cv::Mat &windowRight)
Definition: util2d.cpp:91
rtabmap::util2d::cvtDepthFromFloat
cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat &depth32F)
Definition: util2d.cpp:887
rtabmap::util2d::itemtype
float itemtype
Definition: util2d.cpp:353
rtabmap::Transform::toEigen3f
Eigen::Affine3f toEigen3f() const
Definition: Transform.cpp:344
rtabmap::util2d::getDepth
float RTABMAP_EXP getDepth(const cv::Mat &depthImage, float x, float y, bool smoothing, float depthErrorRatio=0.02f, bool estWithNeighborsIfNull=false)
Definition: util2d.cpp:942
glm::length
GLM_FUNC_DECL genType::value_type length(genType const &x)
rtabmap::util2d::Array3D::x_size
size_t x_size() const
Definition: util2d.cpp:1774
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
rtabmap::util2d::calcStereoCorrespondences
std::vector< cv::Point2f > RTABMAP_EXP calcStereoCorrespondences(const cv::Mat &leftImage, const cv::Mat &rightImage, const std::vector< cv::Point2f > &leftCorners, std::vector< unsigned char > &status, cv::Size winSize=cv::Size(6, 3), int maxLevel=3, int iterations=5, float minDisparity=0.0f, float maxDisparity=64.0f, bool ssdApproach=true)
Definition: util2d.cpp:122
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:240
rtabmap::util2d::Array3D::operator()
Eigen::Vector2f & operator()(const size_t x, const size_t y, const size_t z)
Definition: util2d.cpp:1709
rtabmap::util2d::calcOpticalFlowPyrLKStereo
void RTABMAP_EXP calcOpticalFlowPyrLKStereo(cv::InputArray _prevImg, cv::InputArray _nextImg, cv::InputArray _prevPts, cv::InputOutputArray _nextPts, cv::OutputArray _status, cv::OutputArray _err, cv::Size winSize=cv::Size(15, 3), int maxLevel=3, cv::TermCriteria criteria=cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), int flags=0, double minEigThreshold=1e-4)
Definition: util2d.cpp:367
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:43:40