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 void NMS(
2097  const std::vector<cv::KeyPoint> & ptsIn,
2098  const cv::Mat & descriptorsIn,
2099  std::vector<cv::KeyPoint> & ptsOut,
2100  cv::Mat & descriptorsOut,
2101  int border, int dist_thresh, int img_width, int img_height)
2102 {
2103  std::vector<cv::Point2f> pts_raw;
2104 
2105  for (size_t i = 0; i < ptsIn.size(); i++)
2106  {
2107  int u = (int) ptsIn[i].pt.x;
2108  int v = (int) ptsIn[i].pt.y;
2109 
2110  pts_raw.emplace_back(cv::Point2f(u, v));
2111  }
2112 
2113  //Grid Value Legend:
2114  // 255 : Kept.
2115  // 0 : Empty or suppressed.
2116  // 100 : To be processed (converted to either kept or suppressed).
2117  cv::Mat grid = cv::Mat(cv::Size(img_width, img_height), CV_8UC1);
2118  cv::Mat inds = cv::Mat(cv::Size(img_width, img_height), CV_16UC1);
2119 
2120  cv::Mat confidence = cv::Mat(cv::Size(img_width, img_height), CV_32FC1);
2121  cv::Mat dilated_conf = cv::Mat(cv::Size(img_width, img_height), CV_32FC1);
2122 
2123  grid.setTo(0);
2124  inds.setTo(0);
2125  confidence.setTo(0);
2126 
2127  for (size_t i = 0; i < pts_raw.size(); i++)
2128  {
2129  int uu = (int) pts_raw[i].x;
2130  int vv = (int) pts_raw[i].y;
2131 
2132  grid.at<unsigned char>(vv, uu) = 100;
2133  inds.at<unsigned short>(vv, uu) = i;
2134 
2135  confidence.at<float>(vv, uu) = ptsIn[i].response;
2136  }
2137 
2138  cv::dilate(confidence, dilated_conf, cv::Mat());
2139  cv::Mat peaks = confidence == dilated_conf;
2140 
2141  cv::copyMakeBorder(grid, grid, dist_thresh, dist_thresh, dist_thresh, dist_thresh, cv::BORDER_CONSTANT, 0);
2142 
2143  for (size_t i = 0; i < pts_raw.size(); i++)
2144  {
2145  // account for top left padding
2146  int uu = (int) pts_raw[i].x + dist_thresh;
2147  int vv = (int) pts_raw[i].y + dist_thresh;
2148  float c = confidence.at<float>(vv-dist_thresh, uu-dist_thresh);
2149 
2150  if (grid.at<unsigned char>(vv, uu) == 100) // If not yet suppressed.
2151  {
2152  if (peaks.at<unsigned char>(vv-dist_thresh, uu-dist_thresh) == 255)
2153  {
2154  for(int k = -dist_thresh; k < (dist_thresh+1); k++)
2155  {
2156  for(int j = -dist_thresh; j < (dist_thresh+1); j++)
2157  {
2158  if ((j==0 && k==0) || grid.at<unsigned char>(vv + k, uu + j) == 0)
2159  continue;
2160 
2161  if (confidence.at<float>(vv + k - dist_thresh, uu + j - dist_thresh) <= c)
2162  grid.at<unsigned char>(vv + k, uu + j) = 0;
2163  }
2164  }
2165  grid.at<unsigned char>(vv, uu) = 255;
2166  }
2167  else
2168  {
2169  grid.at<unsigned char>(vv, uu) = 0;
2170  }
2171  }
2172  }
2173 
2174  size_t valid_cnt = 0;
2175  std::vector<int> select_indice;
2176 
2177  grid = cv::Mat(grid, cv::Rect(dist_thresh, dist_thresh, img_width, img_height));
2178 
2179  for (int v = 0; v < img_height; v++)
2180  {
2181  for (int u = 0; u < img_width; u++)
2182  {
2183  if (grid.at<unsigned char>(v,u) == 255)
2184  {
2185  int select_ind = (int) inds.at<unsigned short>(v, u);
2186  ptsOut.emplace_back(ptsIn[select_ind]);
2187  select_indice.emplace_back(select_ind);
2188  valid_cnt++;
2189  }
2190  }
2191  }
2192 
2193  if(!descriptorsIn.empty())
2194  {
2195  UASSERT(descriptorsIn.rows == (int)ptsIn.size());
2196  descriptorsOut.create(select_indice.size(), 256, CV_32F);
2197 
2198  for (size_t i=0; i<select_indice.size(); i++)
2199  {
2200  descriptorsIn.row(select_indice[i]).copyTo(descriptorsOut.row(i));
2201  }
2202  }
2203 }
2204 
2205 std::vector<int> SSC(
2206  const std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, float tolerance, int cols, int rows)
2207 {
2208  // several temp expression variables to simplify solution equation
2209  int exp1 = rows + cols + 2*maxKeypoints;
2210  long long exp2 = ((long long)4*cols + (long long)4*maxKeypoints + (long long)4*rows*maxKeypoints + (long long)rows*rows + (long long)cols*cols - (long long)2*rows*cols + (long long)4*rows*cols*maxKeypoints);
2211  double exp3 = sqrt(exp2);
2212  double exp4 = maxKeypoints - 1;
2213 
2214  double sol1 = -round((exp1 + exp3) / exp4); // first solution
2215  double sol2 = -round((exp1 - exp3) / exp4); // second solution
2216 
2217  // binary search range initialization with positive solution
2218  int high = (sol1 > sol2) ? sol1 : sol2;
2219  int low = floor(sqrt((double)keypoints.size() / maxKeypoints));
2220  low = std::max(1, low);
2221 
2222  int width;
2223  int prevWidth = -1;
2224 
2225  unsigned int Kmin = round(maxKeypoints - (maxKeypoints * tolerance));
2226  unsigned int Kmax = round(maxKeypoints + (maxKeypoints * tolerance));
2227 
2228  std::vector<int> ResultVec, result;
2229  result.reserve(keypoints.size());
2230 
2231  bool complete = false;
2232  while(!complete)
2233  {
2234  width = low + (high - low) / 2;
2235  if(width==prevWidth || low>high) // needed to reassure the same radius is not repeated again
2236  {
2237  ResultVec = result; // return the keypoints from the previous iteration
2238  break;
2239  }
2240  result.clear();
2241  double c = (double)width / 2.0; // initializing Grid
2242  int numCellCols = floor(cols / c);
2243  int numCellRows = floor(rows / c);
2244  std::vector<std::vector<bool>> coveredVec(numCellRows+1, std::vector<bool>(numCellCols+1, false));
2245 
2246  for(unsigned int i=0; i<keypoints.size(); ++i)
2247  {
2248  int row = floor(keypoints[i].pt.y / c); // get position of the cell current point is located at
2249  int col = floor(keypoints[i].pt.x / c);
2250  if(coveredVec[row][col] == false) // if the cell is not covered
2251  {
2252  result.push_back(i);
2253  int rowMin = ((row - floor(width / c)) >= 0) ? (row - floor(width / c)) : 0; // get range which current radius is covering
2254  int rowMax = ((row + floor(width / c)) <= numCellRows) ? (row + floor(width / c)) : numCellRows;
2255  int colMin = ((col - floor(width / c)) >= 0) ? (col - floor(width / c)) : 0;
2256  int colMax = ((col + floor(width / c)) <= numCellCols) ? (col + floor(width / c)) : numCellCols;
2257  for(int rowToCov=rowMin; rowToCov<=rowMax; ++rowToCov)
2258  {
2259  for(int colToCov=colMin; colToCov<=colMax; ++colToCov)
2260  {
2261  if(!coveredVec[rowToCov][colToCov])
2262  coveredVec[rowToCov][colToCov] = true; // cover cells within the square bounding box with width
2263  }
2264  }
2265  }
2266  }
2267 
2268  if(result.size() >= Kmin && result.size() <= Kmax) // solution found
2269  {
2270  ResultVec = result;
2271  complete = true;
2272  }
2273  else if(result.size() < Kmin)
2274  high = width - 1; // update binary search range
2275  else
2276  low = width + 1;
2277  prevWidth = width;
2278  }
2279  return ResultVec;
2280 }
2281 
2282 void rotateImagesUpsideUpIfNecessary(
2283  CameraModel & model,
2284  cv::Mat & rgb,
2285  cv::Mat & depth)
2286 {
2287  float roll,pitch,yaw;
2288  // remove optical rotation
2289  Transform localTransform = model.localTransform()*CameraModel::opticalRotation().inverse();
2290  localTransform.getEulerAngles(roll, pitch, yaw);
2291  UDEBUG("roll=%f pitch=%f yaw=%f", roll, pitch, yaw);
2292  if(fabs(pitch > M_PI/4))
2293  {
2294  // Return original because of ambiguity for what would be considered up...
2295  UDEBUG("Ignoring image rotation as pitch(%f)>Pi/4", pitch);
2296  return;
2297  }
2298  if(roll<0)
2299  {
2300  roll+=2*M_PI;
2301  }
2302  if(roll >= M_PI/4 && roll < 3*M_PI/4)
2303  {
2304  UDEBUG("ROTATION_90 (roll=%f)", roll);
2305  if(!rgb.empty())
2306  {
2307  cv::flip(rgb,rgb,1);
2308  cv::transpose(rgb,rgb);
2309  }
2310  if(!depth.empty())
2311  {
2312  cv::flip(depth,depth,1);
2313  cv::transpose(depth,depth);
2314  }
2315  cv::Size sizet(model.imageHeight(), model.imageWidth());
2316  model = CameraModel(
2317  model.fy(),
2318  model.fx(),
2319  model.cy(),
2320  model.cx()>0?model.imageWidth()-model.cx():0,
2321  model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
2322  model.setImageSize(sizet);
2323  }
2324  else if(roll >= 3*M_PI/4 && roll < 5*M_PI/4)
2325  {
2326  UDEBUG("ROTATION_180 (roll=%f)", roll);
2327  if(!rgb.empty())
2328  {
2329  cv::flip(rgb,rgb,1);
2330  cv::flip(rgb,rgb,0);
2331  }
2332  if(!depth.empty())
2333  {
2334  cv::flip(depth,depth,1);
2335  cv::flip(depth,depth,0);
2336  }
2337  cv::Size sizet(model.imageWidth(), model.imageHeight());
2338  model = CameraModel(
2339  model.fx(),
2340  model.fy(),
2341  model.cx()>0?model.imageWidth()-model.cx():0,
2342  model.cy()>0?model.imageHeight()-model.cy():0,
2343  model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
2344  model.setImageSize(sizet);
2345  }
2346  else if(roll >= 5*M_PI/4 && roll < 7*M_PI/4)
2347  {
2348  UDEBUG("ROTATION_270 (roll=%f)", roll);
2349  if(!rgb.empty())
2350  {
2351  cv::transpose(rgb,rgb);
2352  cv::flip(rgb,rgb,1);
2353  }
2354  if(!depth.empty())
2355  {
2356  cv::transpose(depth,depth);
2357  cv::flip(depth,depth,1);
2358  }
2359  cv::Size sizet(model.imageHeight(), model.imageWidth());
2360  model = CameraModel(
2361  model.fy(),
2362  model.fx(),
2363  model.cy()>0?model.imageHeight()-model.cy():0,
2364  model.cx(),
2365  model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
2366  model.setImageSize(sizet);
2367  }
2368  else
2369  {
2370  UDEBUG("ROTATION_0 (roll=%f)", roll);
2371  }
2372 }
2373 
2374 }
2375 
2376 }
rtabmap::util2d::Array3D::resize
void resize(const size_t width, const size_t height, const size_t depth)
Definition: util2d.cpp:1722
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
int
int
rtabmap::util2d::Array3D::begin
std::vector< Eigen::Vector2f >::iterator begin()
Definition: util2d.cpp:1791
rtabmap::util2d::computeRoi
cv::Rect RTABMAP_CORE_EXPORT computeRoi(const cv::Mat &image, const std::string &roiRatios)
Definition: util2d.cpp:1113
UINFO
#define UINFO(...)
glm::mask
GLM_FUNC_DECL genIType mask(genIType const &count)
uMean
T uMean(const T *v, unsigned int size)
Definition: UMath.h:399
rtabmap::util2d::HSVtoRGB
void RTABMAP_CORE_EXPORT HSVtoRGB(float *r, float *g, float *b, float h, float s, float v)
Definition: util2d.cpp:2047
glm::length
GLM_FUNC_DECL genType::value_type length(genType const &x)
cy
const double cy
b1
Vector2 b1(2, -1)
rtabmap::util2d::fillRegisteredDepthHoles
void RTABMAP_CORE_EXPORT fillRegisteredDepthHoles(cv::Mat &depthRegistered, bool vertical, bool horizontal, bool fillDoubleHoles=false)
Definition: util2d.cpp:1598
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
D
MatrixXcd D
set
x1
x1
v1
v1
rtabmap::util2d::interpolate
cv::Mat RTABMAP_CORE_EXPORT interpolate(const cv::Mat &image, int factor, float depthErrorRatio=0.02f)
Definition: util2d.cpp:1250
Eigen::Transform
alpha
RealScalar alpha
timer
s
RealScalar s
rtabmap::util2d::sad
float RTABMAP_CORE_EXPORT sad(const cv::Mat &windowLeft, const cv::Mat &windowRight)
Definition: util2d.cpp:91
b
Array< int, 3, 1 > b
rtflann::uchar
unsigned char uchar
Definition: matrix.h:69
fx
const double fx
c
Scalar Scalar * c
h
const double h
size
Index size
rtabmap::CameraModel
Definition: CameraModel.h:38
b2
Vector2 b2(4, -5)
count
Index count
rtabmap::util2d::Array3D::y_size
size_t y_size() const
Definition: util2d.cpp:1783
col
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ColXpr col(Index i) const
A11
Matrix A11
type
rtabmap::util2d::itemtype
float itemtype
Definition: util2d.cpp:357
rtabmap::util2d::registerDepth
cv::Mat RTABMAP_CORE_EXPORT 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
y
Matrix3f y
buffer
rtabmap::util2d::brightnessAndContrastAuto
cv::Mat RTABMAP_CORE_EXPORT 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
uVariance
T uVariance(const T *v, unsigned int size, T meanV)
Definition: UMath.h:489
rtabmap::util2d::cvtDepthToFloat
cv::Mat RTABMAP_CORE_EXPORT cvtDepthToFloat(const cv::Mat &depth16U)
Definition: util2d.cpp:928
rtabmap::util2d::Array3D::end
std::vector< Eigen::Vector2f >::iterator end()
Definition: util2d.cpp:1795
rtabmap::util2d::disparityFromStereoImages
cv::Mat RTABMAP_CORE_EXPORT disparityFromStereoImages(const cv::Mat &leftImage, const cv::Mat &rightImage, const ParametersMap &parameters=ParametersMap())
Definition: util2d.cpp:738
rtabmap::util2d::rotateImagesUpsideUpIfNecessary
void RTABMAP_CORE_EXPORT rotateImagesUpsideUpIfNecessary(CameraModel &model, cv::Mat &rgb, cv::Mat &depth)
Rotate images and camera model so that the top of the image is up.
Definition: util2d.cpp:2282
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
beta
double beta(double a, double b)
rtabmap::util2d::Array3D::y_dim_
size_t y_dim_
Definition: util2d.cpp:1808
glm::floor
GLM_FUNC_DECL genType floor(genType const &x)
rows
int rows
UTimer.h
rtabmap::util2d::decimate
cv::Mat RTABMAP_CORE_EXPORT decimate(const cv::Mat &image, int d)
Definition: util2d.cpp:1201
glm::round
GLM_FUNC_DECL genType round(genType const &x)
UMath.h
Basic mathematics functions.
rtabmap::util2d::Array3D::v_
std::vector< Eigen::Vector2f > v_
Definition: util2d.cpp:1807
fabs
Real fabs(const Real &a)
rtabmap::util2d::Array3D::z_size
size_t z_size() const
Definition: util2d.cpp:1787
range
Double_ range(const Point2_ &p, const Point2_ &q)
rtabmap::util2d::fillDepthHoles
cv::Mat RTABMAP_CORE_EXPORT fillDepthHoles(const cv::Mat &depth, int maximumHoleSize=1, float errorRatio=0.02f)
Definition: util2d.cpp:1439
rtabmap::Transform::getEulerAngles
void getEulerAngles(float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:263
J
JacobiRotation< float > J
data
int data[]
delta
def delta(g0, g1)
util3d_transforms.h
g
float g
j
std::ptrdiff_t j
rtabmap::util2d::fastBilateralFiltering
cv::Mat RTABMAP_CORE_EXPORT fastBilateralFiltering(const cv::Mat &depth, float sigmaS=15.0f, float sigmaR=0.05f, bool earlyDivision=false)
Definition: util2d.cpp:1814
q
EIGEN_DEVICE_FUNC const Scalar & q
glm::exp2
GLM_FUNC_DECL genType exp2(genType const &x)
UConversion.h
Some conversion functions.
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::StereoDense
Definition: StereoDense.h:38
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
x2
x2
std::swap
void swap(GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &a, GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &b)
glm::row
GLM_FUNC_DECL genType::row_type row(genType const &m, length_t const &index)
rtabmap::util2d::calcStereoCorrespondences
std::vector< cv::Point2f > RTABMAP_CORE_EXPORT 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
step
def step(data, isam, result, truth, currPoseIndex, isamArgs=())
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
UASSERT
#define UASSERT(condition)
d
d
z
z
level
level
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:238
rtabmap::util2d::Array3D::begin
std::vector< Eigen::Vector2f >::const_iterator begin() const
Definition: util2d.cpp:1799
x
x
m
Matrix3f m
error
void error(const char *str)
p
Point3_ p(2)
rtabmap::util2d::ssd
float RTABMAP_CORE_EXPORT ssd(const cv::Mat &windowLeft, const cv::Mat &windowRight)
Definition: util2d.cpp:56
rtabmap::CameraModel::opticalRotation
static Transform opticalRotation()
Definition: CameraModel.h:45
rtabmap::util2d::depthFromDisparity
cv::Mat RTABMAP_CORE_EXPORT depthFromDisparity(const cv::Mat &disparity, float fx, float baseline, int type=CV_32FC1)
Definition: util2d.cpp:763
rtabmap::util2d::getDepth
float RTABMAP_CORE_EXPORT getDepth(const cv::Mat &depthImage, float x, float y, bool smoothing, float depthErrorRatio=0.02f, bool estWithNeighborsIfNull=false)
Definition: util2d.cpp:947
uIsInBounds
bool uIsInBounds(const T &value, const T &low, const T &high)
Definition: UMath.h:932
rtabmap::util2d::Array3D::end
std::vector< Eigen::Vector2f >::const_iterator end() const
Definition: util2d.cpp:1803
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
out
std::ofstream out("Result.txt")
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
offset
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate offset
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
UWARN
#define UWARN(...)
CV_DESCALE
#define CV_DESCALE(x, n)
Definition: util2d.cpp:358
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::util2d::Array3D::Array3D
Array3D(const size_t width, const size_t height, const size_t depth)
Definition: util2d.cpp:1705
M_PI
#define M_PI
Definition: tango-gl/include/tango-gl/util.h:66
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
ULogger.h
ULogger class and convenient macros.
a
ArrayXXi a
rtabmap::Transform
Definition: Transform.h:41
util2d.h
rtabmap::util2d::cvtDepthFromFloat
cv::Mat RTABMAP_CORE_EXPORT cvtDepthFromFloat(const cv::Mat &depth32F)
Definition: util2d.cpp:892
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:564
rtabmap::StereoDense::computeDisparity
virtual cv::Mat computeDisparity(const cv::Mat &leftImage, const cv::Mat &rightImage) const =0
rtabmap::util2d::Array3D::z_dim_
size_t z_dim_
Definition: util2d.cpp:1808
rtabmap::util2d::calcOpticalFlowPyrLKStereo
void RTABMAP_CORE_EXPORT 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
rtabmap::util2d::Array3D::x_size
size_t x_size() const
Definition: util2d.cpp:1779
rtabmap::util2d::Array3D
Definition: util2d.cpp:1702
values
leaf::MyValues values
rtabmap::util2d::depthFromStereoCorrespondences
cv::Mat RTABMAP_CORE_EXPORT 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
iter
iterator iter(handle obj)
glm::abs
GLM_FUNC_DECL genType abs(genType const &x)
dim
dim
c_str
const char * c_str(Args &&...args)
A22
Matrix A22
UStl.h
Wrappers of STL for convenient functions.
diff
diff
v
Array< int, Dynamic, 1 > v
UDEBUG
#define UDEBUG(...)
UTimer
Definition: UTimer.h:46
rtabmap::util2d::Array3D::x_dim_
size_t x_dim_
Definition: util2d.cpp:1808
rtabmap::util2d::NMS
void RTABMAP_CORE_EXPORT NMS(const std::vector< cv::KeyPoint > &ptsIn, const cv::Mat &descriptorsIn, std::vector< cv::KeyPoint > &ptsOut, cv::Mat &descriptorsOut, int border, int dist_thresh, int img_width, int img_height)
Definition: util2d.cpp:2096
float
float
rtabmap::StereoDense::create
static StereoDense * create(const ParametersMap &parameters)
Definition: StereoDense.cpp:33
rtabmap::util2d::SSC
std::vector< int > RTABMAP_CORE_EXPORT SSC(const std::vector< cv::KeyPoint > &keypoints, int maxKeypoints, float tolerance, int cols, int rows)
Definition: util2d.cpp:2205
glm::proj
GLM_FUNC_DECL vecType proj(vecType const &x, vecType const &Normal)
rtabmap::util2d::acctype
float acctype
Definition: util2d.cpp:356
I
I
rtabmap::util2d::exposureFusion
cv::Mat RTABMAP_CORE_EXPORT exposureFusion(const std::vector< cv::Mat > &images)
Definition: util2d.cpp:2027
rtabmap::util2d::Array3D::operator()
Eigen::Vector2f & operator()(const size_t x, const size_t y, const size_t z)
Definition: util2d.cpp:1714
transform
EIGEN_DONT_INLINE void transform(const Quaternion< Scalar > &t, Data &data)
cx
const double cx
cols
int cols
uStr2Float
float UTILITE_EXPORT uStr2Float(const std::string &str)
Definition: UConversion.cpp:138
rtabmap::util2d::depthFromStereoImages
cv::Mat RTABMAP_CORE_EXPORT 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
t
Point2 t(10, 10)
dst
char * dst
Definition: lz4.h:354
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
rtabmap::util2d::disparityFromStereoCorrespondences
cv::Mat RTABMAP_CORE_EXPORT 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
value
value
i
int i
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:53
result
RESULT & result
StereoDense.h
baseline
double baseline
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
fy
const double fy
v2
v2
cvtColor
CvImagePtr cvtColor(const CvImageConstPtr &source, const std::string &encoding)
rtabmap::util2d::Array3D::trilinear_interpolation
Eigen::Vector2f trilinear_interpolation(const float x, const float y, const float z)
Definition: util2d.cpp:1731

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:23