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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:38:58