00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/core/util2d.h"
00029
00030 #include <rtabmap/utilite/ULogger.h>
00031 #include <rtabmap/utilite/UMath.h>
00032 #include <rtabmap/utilite/UConversion.h>
00033 #include <rtabmap/utilite/UTimer.h>
00034 #include <rtabmap/utilite/UStl.h>
00035 #include <rtabmap/core/util3d_transforms.h>
00036 #include <rtabmap/core/StereoDense.h>
00037 #include <opencv2/calib3d/calib3d.hpp>
00038 #include <opencv2/imgproc/imgproc.hpp>
00039 #include <opencv2/video/tracking.hpp>
00040 #include <opencv2/highgui/highgui.hpp>
00041 #include <opencv2/imgproc/types_c.h>
00042 #include <map>
00043 #include <Eigen/Core>
00044
00045 #if CV_MAJOR_VERSION >= 3
00046 #include <opencv2/photo/photo.hpp>
00047 #endif
00048
00049 namespace rtabmap
00050 {
00051
00052 namespace util2d
00053 {
00054
00055
00056 float ssd(const cv::Mat & windowLeft, const cv::Mat & windowRight)
00057 {
00058 UASSERT_MSG(windowLeft.type() == CV_8UC1 || windowLeft.type() == CV_32FC1 || windowLeft.type() == CV_16SC2, uFormat("Type=%d", windowLeft.type()).c_str());
00059 UASSERT(windowLeft.type() == windowRight.type());
00060 UASSERT_MSG(windowLeft.rows == windowRight.rows, uFormat("%d vs %d", windowLeft.rows, windowRight.rows).c_str());
00061 UASSERT_MSG(windowLeft.cols == windowRight.cols, uFormat("%d vs %d", windowLeft.cols, windowRight.cols).c_str());
00062
00063 float score = 0.0f;
00064 for(int v=0; v<windowLeft.rows; ++v)
00065 {
00066 for(int u=0; u<windowLeft.cols; ++u)
00067 {
00068 float s = 0.0f;
00069 if(windowLeft.type() == CV_8UC1)
00070 {
00071 s = float(windowLeft.at<unsigned char>(v,u))-float(windowRight.at<unsigned char>(v,u));
00072 }
00073 else if(windowLeft.type() == CV_32FC1)
00074 {
00075 s = windowLeft.at<float>(v,u)-windowRight.at<float>(v,u);
00076 }
00077 else if(windowLeft.type() == CV_16SC2)
00078 {
00079 float sL = float(windowLeft.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowLeft.at<cv::Vec2s>(v,u)[1])*0.5f;
00080 float sR = float(windowRight.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowRight.at<cv::Vec2s>(v,u)[1])*0.5f;
00081 s = sL - sR;
00082 }
00083
00084 score += s*s;
00085 }
00086 }
00087 return score;
00088 }
00089
00090
00091 float sad(const cv::Mat & windowLeft, const cv::Mat & windowRight)
00092 {
00093 UASSERT_MSG(windowLeft.type() == CV_8UC1 || windowLeft.type() == CV_32FC1 || windowLeft.type() == CV_16SC2, uFormat("Type=%d", windowLeft.type()).c_str());
00094 UASSERT(windowLeft.type() == windowRight.type());
00095 UASSERT_MSG(windowLeft.rows == windowRight.rows, uFormat("%d vs %d", windowLeft.rows, windowRight.rows).c_str());
00096 UASSERT_MSG(windowLeft.cols == windowRight.cols, uFormat("%d vs %d", windowLeft.cols, windowRight.cols).c_str());
00097
00098 float score = 0.0f;
00099 for(int v=0; v<windowLeft.rows; ++v)
00100 {
00101 for(int u=0; u<windowLeft.cols; ++u)
00102 {
00103 if(windowLeft.type() == CV_8UC1)
00104 {
00105 score += fabs(float(windowLeft.at<unsigned char>(v,u))-float(windowRight.at<unsigned char>(v,u)));
00106 }
00107 else if(windowLeft.type() == CV_32FC1)
00108 {
00109 score += fabs(windowLeft.at<float>(v,u)-windowRight.at<float>(v,u));
00110 }
00111 else if(windowLeft.type() == CV_16SC2)
00112 {
00113 float sL = float(windowLeft.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowLeft.at<cv::Vec2s>(v,u)[1])*0.5f;
00114 float sR = float(windowRight.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowRight.at<cv::Vec2s>(v,u)[1])*0.5f;
00115 score += fabs(sL - sR);
00116 }
00117 }
00118 }
00119 return score;
00120 }
00121
00122 std::vector<cv::Point2f> calcStereoCorrespondences(
00123 const cv::Mat & leftImage,
00124 const cv::Mat & rightImage,
00125 const std::vector<cv::Point2f> & leftCorners,
00126 std::vector<unsigned char> & status,
00127 cv::Size winSize,
00128 int maxLevel,
00129 int iterations,
00130 float minDisparityF,
00131 float maxDisparityF,
00132 bool ssdApproach)
00133 {
00134 UDEBUG("winSize=(%d,%d)", winSize.width, winSize.height);
00135 UDEBUG("maxLevel=%d", maxLevel);
00136 UDEBUG("minDisparity=%f", minDisparityF);
00137 UDEBUG("maxDisparity=%f", maxDisparityF);
00138 UDEBUG("iterations=%d", iterations);
00139 UDEBUG("ssdApproach=%d", ssdApproach?1:0);
00140
00141
00142 if(winSize.width%2 == 0)
00143 {
00144 winSize.width+=1;
00145 }
00146 if(winSize.height%2 == 0)
00147 {
00148 winSize.height+=1;
00149 }
00150
00151 cv::Size halfWin((winSize.width-1)/2, (winSize.height-1)/2);
00152
00153 UTimer timer;
00154 double pyramidTime = 0.0;
00155 double disparityTime = 0.0;
00156 double subpixelTime = 0.0;
00157
00158 std::vector<cv::Point2f> rightCorners(leftCorners.size());
00159 std::vector<cv::Mat> leftPyramid, rightPyramid;
00160 maxLevel = cv::buildOpticalFlowPyramid( leftImage, leftPyramid, winSize, maxLevel, false);
00161 maxLevel = cv::buildOpticalFlowPyramid( rightImage, rightPyramid, winSize, maxLevel, false);
00162 pyramidTime = timer.ticks();
00163
00164 status = std::vector<unsigned char>(leftCorners.size(), 0);
00165 int totalIterations = 0;
00166 int noSubPixel = 0;
00167 int added = 0;
00168 int minDisparity = std::floor(minDisparityF);
00169 int maxDisparity = std::floor(maxDisparityF);
00170 for(unsigned int i=0; i<leftCorners.size(); ++i)
00171 {
00172 int oi=0;
00173 float bestScore = -1.0f;
00174 int bestScoreIndex = -1;
00175 int tmpMinDisparity = minDisparity;
00176 int tmpMaxDisparity = maxDisparity;
00177
00178 int iterations = 0;
00179 for(int level=maxLevel; level>=0; --level)
00180 {
00181 UASSERT(level < (int)leftPyramid.size());
00182
00183 cv::Point2i center(int(leftCorners[i].x/float(1<<level)), int(leftCorners[i].y/float(1<<level)));
00184
00185 oi=0;
00186 bestScore = -1.0f;
00187 bestScoreIndex = -1;
00188 int localMaxDisparity = -tmpMaxDisparity / (1<<level);
00189 int localMinDisparity = -tmpMinDisparity / (1<<level);
00190
00191 if(center.x-halfWin.width-(level==0?1:0) >=0 && center.x+halfWin.width+(level==0?1:0) < leftPyramid[level].cols &&
00192 center.y-halfWin.height >=0 && center.y+halfWin.height < leftPyramid[level].rows)
00193 {
00194 cv::Mat windowLeft(leftPyramid[level],
00195 cv::Range(center.y-halfWin.height,center.y+halfWin.height+1),
00196 cv::Range(center.x-halfWin.width,center.x+halfWin.width+1));
00197 int minCol = center.x+localMaxDisparity-halfWin.width-1;
00198 if(minCol < 0)
00199 {
00200 localMaxDisparity -= minCol;
00201 }
00202
00203 int maxCol = center.x+localMinDisparity+halfWin.width+1;
00204 if(maxCol >= leftPyramid[level].cols)
00205 {
00206 localMinDisparity += maxCol-leftPyramid[level].cols-1;
00207 }
00208
00209 if(localMinDisparity < localMaxDisparity)
00210 {
00211 localMaxDisparity = localMinDisparity;
00212 }
00213 int length = localMinDisparity-localMaxDisparity+1;
00214 std::vector<float> scores = std::vector<float>(length, 0.0f);
00215
00216 for(int d=localMinDisparity; d>localMaxDisparity; --d)
00217 {
00218 ++iterations;
00219 cv::Mat windowRight(rightPyramid[level],
00220 cv::Range(center.y-halfWin.height,center.y+halfWin.height+1),
00221 cv::Range(center.x+d-halfWin.width,center.x+d+halfWin.width+1));
00222 scores[oi] = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
00223 if(scores[oi] > 0 && (bestScore < 0.0f || scores[oi] < bestScore))
00224 {
00225 bestScoreIndex = oi;
00226 bestScore = scores[oi];
00227 }
00228 ++oi;
00229 }
00230
00231 if(bestScoreIndex>=0)
00232 {
00233 if(level>0)
00234 {
00235 tmpMaxDisparity = tmpMinDisparity+(bestScoreIndex+1)*(1<<level);
00236 tmpMaxDisparity+=tmpMaxDisparity%level;
00237 if(tmpMaxDisparity > maxDisparity)
00238 {
00239 tmpMaxDisparity = maxDisparity;
00240 }
00241 tmpMinDisparity = tmpMinDisparity+(bestScoreIndex-1)*(1<<level);
00242 tmpMinDisparity -= tmpMinDisparity%level;
00243 if(tmpMinDisparity < minDisparity)
00244 {
00245 tmpMinDisparity = minDisparity;
00246 }
00247 }
00248 }
00249 }
00250 }
00251 disparityTime+=timer.ticks();
00252 totalIterations+=iterations;
00253
00254 if(bestScoreIndex>=0)
00255 {
00256
00257 int d = -(tmpMinDisparity+bestScoreIndex);
00258
00259 cv::Mat windowLeft(winSize, CV_32FC1);
00260 cv::Mat windowRight(winSize, CV_32FC1);
00261 cv::getRectSubPix(leftPyramid[0],
00262 winSize,
00263 leftCorners[i],
00264 windowLeft,
00265 windowLeft.type());
00266 if(leftCorners[i].x != float(int(leftCorners[i].x)))
00267 {
00268
00269 cv::getRectSubPix(rightPyramid[0],
00270 winSize,
00271 cv::Point2f(leftCorners[i].x+float(d), leftCorners[i].y),
00272 windowRight,
00273 windowRight.type());
00274 bestScore = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
00275 }
00276
00277 float xc = leftCorners[i].x+float(d);
00278 float vc = bestScore;
00279 float step = 0.5f;
00280 std::map<float, float> cache;
00281 bool reject = false;
00282 for(int it=0; it<iterations; ++it)
00283 {
00284 float x1 = xc-step;
00285 float x2 = xc+step;
00286 float v1 = uValue(cache, x1, 0.0f);
00287 float v2 = uValue(cache, x2, 0.0f);
00288 if(v1 == 0.0f)
00289 {
00290 cv::getRectSubPix(rightPyramid[0],
00291 winSize,
00292 cv::Point2f(x1, leftCorners[i].y),
00293 windowRight,
00294 windowRight.type());
00295 v1 = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
00296 }
00297 if(v2 == 0.0f)
00298 {
00299 cv::getRectSubPix(rightPyramid[0],
00300 winSize,
00301 cv::Point2f(x2, leftCorners[i].y),
00302 windowRight,
00303 windowRight.type());
00304 v2 = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
00305 }
00306
00307 float previousXc = xc;
00308 float previousVc = vc;
00309
00310 xc = v1<vc&&v1<v2?x1:v2<vc&&v2<v1?x2:xc;
00311 vc = v1<vc&&v1<v2?v1:v2<vc&&v2<v1?v2:vc;
00312
00313 if(previousXc == xc)
00314 {
00315 step /= 2.0f;
00316 }
00317 else
00318 {
00319 cache.insert(std::make_pair(previousXc, previousVc));
00320 }
00321
00322 if(
00323 float(leftCorners[i].x - xc) <= minDisparityF)
00324 {
00325 reject = true;
00326 break;
00327 }
00328 }
00329
00330 rightCorners[i] = cv::Point2f(xc, leftCorners[i].y);
00331 status[i] = reject?0:1;
00332 if(!reject)
00333 {
00334 if(leftCorners[i].x+float(d) != xc)
00335 {
00336 ++noSubPixel;
00337 }
00338 ++added;
00339 }
00340 }
00341 subpixelTime+=timer.ticks();
00342 }
00343 UDEBUG("SubPixel=%d/%d added (total=%d)", noSubPixel, added, (int)status.size());
00344 UDEBUG("totalIterations=%d", totalIterations);
00345 UDEBUG("Time pyramid = %f s", pyramidTime);
00346 UDEBUG("Time disparity = %f s", disparityTime);
00347 UDEBUG("Time sub-pixel = %f s", subpixelTime);
00348
00349 return rightCorners;
00350 }
00351
00352 typedef float acctype;
00353 typedef float itemtype;
00354 #define CV_DESCALE(x,n) (((x) + (1 << ((n)-1))) >> (n))
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367 void calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputArray _nextImg,
00368 cv::InputArray _prevPts, cv::InputOutputArray _nextPts,
00369 cv::OutputArray _status, cv::OutputArray _err,
00370 cv::Size winSize, int maxLevel,
00371 cv::TermCriteria criteria,
00372 int flags, double minEigThreshold )
00373 {
00374 cv::Mat prevPtsMat = _prevPts.getMat();
00375 const int derivDepth = cv::DataType<short>::depth;
00376
00377 CV_Assert( maxLevel >= 0 && winSize.width > 2 && winSize.height > 2 );
00378
00379 int level=0, i, npoints;
00380 CV_Assert( (npoints = prevPtsMat.checkVector(2, CV_32F, true)) >= 0 );
00381
00382 if( npoints == 0 )
00383 {
00384 _nextPts.release();
00385 _status.release();
00386 _err.release();
00387 return;
00388 }
00389
00390 if( !(flags & cv::OPTFLOW_USE_INITIAL_FLOW) )
00391 _nextPts.create(prevPtsMat.size(), prevPtsMat.type(), -1, true);
00392
00393 cv::Mat nextPtsMat = _nextPts.getMat();
00394 CV_Assert( nextPtsMat.checkVector(2, CV_32F, true) == npoints );
00395
00396 const cv::Point2f* prevPts = prevPtsMat.ptr<cv::Point2f>();
00397 cv::Point2f* nextPts = nextPtsMat.ptr<cv::Point2f>();
00398
00399 _status.create((int)npoints, 1, CV_8U, -1, true);
00400 cv::Mat statusMat = _status.getMat(), errMat;
00401 CV_Assert( statusMat.isContinuous() );
00402 uchar* status = statusMat.ptr();
00403 float* err = 0;
00404
00405 for( i = 0; i < npoints; i++ )
00406 status[i] = true;
00407
00408 if( _err.needed() )
00409 {
00410 _err.create((int)npoints, 1, CV_32F, -1, true);
00411 errMat = _err.getMat();
00412 CV_Assert( errMat.isContinuous() );
00413 err = errMat.ptr<float>();
00414 }
00415
00416 std::vector<cv::Mat> prevPyr, nextPyr;
00417 int levels1 = -1;
00418 int lvlStep1 = 1;
00419 int levels2 = -1;
00420 int lvlStep2 = 1;
00421
00422
00423 if(_prevImg.kind() != cv::_InputArray::STD_VECTOR_MAT)
00424 {
00425
00426 maxLevel = cv::buildOpticalFlowPyramid(_prevImg, prevPyr, winSize, maxLevel, true);
00427 }
00428 else if(_prevImg.kind() == cv::_InputArray::STD_VECTOR_MAT)
00429 {
00430 _prevImg.getMatVector(prevPyr);
00431 }
00432
00433 levels1 = int(prevPyr.size()) - 1;
00434 CV_Assert(levels1 >= 0);
00435
00436 if (levels1 % 2 == 1 && prevPyr[0].channels() * 2 == prevPyr[1].channels() && prevPyr[1].depth() == derivDepth)
00437 {
00438 lvlStep1 = 2;
00439 levels1 /= 2;
00440 }
00441
00442
00443 if(levels1 > 0)
00444 {
00445 cv::Size fullSize;
00446 cv::Point ofs;
00447 prevPyr[lvlStep1].locateROI(fullSize, ofs);
00448 CV_Assert(ofs.x >= winSize.width && ofs.y >= winSize.height
00449 && ofs.x + prevPyr[lvlStep1].cols + winSize.width <= fullSize.width
00450 && ofs.y + prevPyr[lvlStep1].rows + winSize.height <= fullSize.height);
00451 }
00452
00453 if(levels1 < maxLevel)
00454 maxLevel = levels1;
00455
00456 if(_nextImg.kind() != cv::_InputArray::STD_VECTOR_MAT)
00457 {
00458
00459 maxLevel = cv::buildOpticalFlowPyramid(_nextImg, nextPyr, winSize, maxLevel, false);
00460 }
00461 else if(_nextImg.kind() == cv::_InputArray::STD_VECTOR_MAT)
00462 {
00463 _nextImg.getMatVector(nextPyr);
00464 }
00465
00466 levels2 = int(nextPyr.size()) - 1;
00467 CV_Assert(levels2 >= 0);
00468
00469 if (levels2 % 2 == 1 && nextPyr[0].channels() * 2 == nextPyr[1].channels() && nextPyr[1].depth() == derivDepth)
00470 {
00471 lvlStep2 = 2;
00472 levels2 /= 2;
00473 }
00474
00475
00476 if(levels2 > 0)
00477 {
00478 cv::Size fullSize;
00479 cv::Point ofs;
00480 nextPyr[lvlStep2].locateROI(fullSize, ofs);
00481 CV_Assert(ofs.x >= winSize.width && ofs.y >= winSize.height
00482 && ofs.x + nextPyr[lvlStep2].cols + winSize.width <= fullSize.width
00483 && ofs.y + nextPyr[lvlStep2].rows + winSize.height <= fullSize.height);
00484 }
00485
00486 if(levels2 < maxLevel)
00487 maxLevel = levels2;
00488
00489 if( (criteria.type & cv::TermCriteria::COUNT) == 0 )
00490 criteria.maxCount = 30;
00491 else
00492 criteria.maxCount = std::min(std::max(criteria.maxCount, 0), 100);
00493 if( (criteria.type & cv::TermCriteria::EPS) == 0 )
00494 criteria.epsilon = 0.01;
00495 else
00496 criteria.epsilon = std::min(std::max(criteria.epsilon, 0.), 10.);
00497 criteria.epsilon *= criteria.epsilon;
00498
00499
00500 for( level = maxLevel; level >= 0; level-- )
00501 {
00502 cv::Mat derivI = prevPyr[level * lvlStep1 + 1];
00503
00504 CV_Assert(prevPyr[level * lvlStep1].size() == nextPyr[level * lvlStep2].size());
00505 CV_Assert(prevPyr[level * lvlStep1].type() == nextPyr[level * lvlStep2].type());
00506
00507 const cv::Mat & prevImg = prevPyr[level * lvlStep1];
00508 const cv::Mat & prevDeriv = derivI;
00509 const cv::Mat & nextImg = nextPyr[level * lvlStep2];
00510
00511
00512 {
00513 cv::Point2f halfWin((winSize.width-1)*0.5f, (winSize.height-1)*0.5f);
00514 const cv::Mat& I = prevImg;
00515 const cv::Mat& J = nextImg;
00516 const cv::Mat& derivI = prevDeriv;
00517
00518 int j, cn = I.channels(), cn2 = cn*2;
00519 cv::AutoBuffer<short> _buf(winSize.area()*(cn + cn2));
00520 int derivDepth = cv::DataType<short>::depth;
00521
00522 cv::Mat IWinBuf(winSize, CV_MAKETYPE(derivDepth, cn), (short*)_buf);
00523 cv::Mat derivIWinBuf(winSize, CV_MAKETYPE(derivDepth, cn2), (short*)_buf + winSize.area()*cn);
00524
00525 for( int ptidx = 0; ptidx < npoints; ptidx++ )
00526 {
00527 cv::Point2f prevPt = prevPts[ptidx]*(float)(1./(1 << level));
00528 cv::Point2f nextPt;
00529 if( level == maxLevel )
00530 {
00531 if( flags & cv::OPTFLOW_USE_INITIAL_FLOW )
00532 nextPt = nextPts[ptidx]*(float)(1./(1 << level));
00533 else
00534 nextPt = prevPt;
00535 }
00536 else
00537 nextPt = nextPts[ptidx]*2.f;
00538 nextPts[ptidx] = nextPt;
00539
00540 cv::Point2i iprevPt, inextPt;
00541 prevPt -= halfWin;
00542 iprevPt.x = cvFloor(prevPt.x);
00543 iprevPt.y = cvFloor(prevPt.y);
00544
00545 if( iprevPt.x < -winSize.width || iprevPt.x >= derivI.cols ||
00546 iprevPt.y < -winSize.height || iprevPt.y >= derivI.rows )
00547 {
00548 if( level == 0 )
00549 {
00550 if( status )
00551 status[ptidx] = false;
00552 if( err )
00553 err[ptidx] = 0;
00554 }
00555 continue;
00556 }
00557
00558 float a = prevPt.x - iprevPt.x;
00559 float b = prevPt.y - iprevPt.y;
00560 const int W_BITS = 14, W_BITS1 = 14;
00561 const float FLT_SCALE = 1.f/(1 << 20);
00562 int iw00 = cvRound((1.f - a)*(1.f - b)*(1 << W_BITS));
00563 int iw01 = cvRound(a*(1.f - b)*(1 << W_BITS));
00564 int iw10 = cvRound((1.f - a)*b*(1 << W_BITS));
00565 int iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
00566
00567 int dstep = (int)(derivI.step/derivI.elemSize1());
00568 int stepI = (int)(I.step/I.elemSize1());
00569 int stepJ = (int)(J.step/J.elemSize1());
00570 acctype iA11 = 0, iA12 = 0, iA22 = 0;
00571 float A11, A12, A22;
00572
00573
00574 int x, y;
00575 for( y = 0; y < winSize.height; y++ )
00576 {
00577 const uchar* src = I.ptr() + (y + iprevPt.y)*stepI + iprevPt.x*cn;
00578 const short* dsrc = derivI.ptr<short>() + (y + iprevPt.y)*dstep + iprevPt.x*cn2;
00579
00580 short* Iptr = IWinBuf.ptr<short>(y);
00581 short* dIptr = derivIWinBuf.ptr<short>(y);
00582
00583 x = 0;
00584
00585 for( ; x < winSize.width*cn; x++, dsrc += 2, dIptr += 2 )
00586 {
00587 int ival = CV_DESCALE(src[x]*iw00 + src[x+cn]*iw01 +
00588 src[x+stepI]*iw10 + src[x+stepI+cn]*iw11, W_BITS1-5);
00589 int ixval = CV_DESCALE(dsrc[0]*iw00 + dsrc[cn2]*iw01 +
00590 dsrc[dstep]*iw10 + dsrc[dstep+cn2]*iw11, W_BITS1);
00591 int iyval = CV_DESCALE(dsrc[1]*iw00 + dsrc[cn2+1]*iw01 + dsrc[dstep+1]*iw10 +
00592 dsrc[dstep+cn2+1]*iw11, W_BITS1);
00593
00594 Iptr[x] = (short)ival;
00595 dIptr[0] = (short)ixval;
00596 dIptr[1] = (short)iyval;
00597
00598 iA11 += (itemtype)(ixval*ixval);
00599 iA12 += (itemtype)(ixval*iyval);
00600 iA22 += (itemtype)(iyval*iyval);
00601 }
00602 }
00603
00604 A11 = iA11*FLT_SCALE;
00605 A12 = iA12*FLT_SCALE;
00606 A22 = iA22*FLT_SCALE;
00607
00608 float D = A11*A22 - A12*A12;
00609 float minEig = (A22 + A11 - std::sqrt((A11-A22)*(A11-A22) +
00610 4.f*A12*A12))/(2*winSize.width*winSize.height);
00611
00612 if( err && (flags & cv::OPTFLOW_LK_GET_MIN_EIGENVALS) != 0 )
00613 err[ptidx] = (float)minEig;
00614
00615 if( minEig < minEigThreshold || D < FLT_EPSILON )
00616 {
00617 if( level == 0 && status )
00618 status[ptidx] = false;
00619 continue;
00620 }
00621
00622 D = 1.f/D;
00623
00624 nextPt -= halfWin;
00625 cv::Point2f prevDelta;
00626
00627 for( j = 0; j < criteria.maxCount; j++ )
00628 {
00629 inextPt.x = cvFloor(nextPt.x);
00630 inextPt.y = cvFloor(nextPt.y);
00631
00632 if( inextPt.x < -winSize.width || inextPt.x >= J.cols ||
00633 inextPt.y < -winSize.height || inextPt.y >= J.rows )
00634 {
00635 if( level == 0 && status )
00636 status[ptidx] = false;
00637 break;
00638 }
00639
00640 a = nextPt.x - inextPt.x;
00641 b = nextPt.y - inextPt.y;
00642 iw00 = cvRound((1.f - a)*(1.f - b)*(1 << W_BITS));
00643 iw01 = cvRound(a*(1.f - b)*(1 << W_BITS));
00644 iw10 = cvRound((1.f - a)*b*(1 << W_BITS));
00645 iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
00646 acctype ib1 = 0, ib2 = 0;
00647 float b1, b2;
00648
00649 for( y = 0; y < winSize.height; y++ )
00650 {
00651 const uchar* Jptr = J.ptr() + (y + inextPt.y)*stepJ + inextPt.x*cn;
00652 const short* Iptr = IWinBuf.ptr<short>(y);
00653 const short* dIptr = derivIWinBuf.ptr<short>(y);
00654
00655 x = 0;
00656
00657 for( ; x < winSize.width*cn; x++, dIptr += 2 )
00658 {
00659 int diff = CV_DESCALE(Jptr[x]*iw00 + Jptr[x+cn]*iw01 +
00660 Jptr[x+stepJ]*iw10 + Jptr[x+stepJ+cn]*iw11,
00661 W_BITS1-5) - Iptr[x];
00662 ib1 += (itemtype)(diff*dIptr[0]);
00663 ib2 += (itemtype)(diff*dIptr[1]);
00664 }
00665 }
00666
00667 b1 = ib1*FLT_SCALE;
00668 b2 = ib2*FLT_SCALE;
00669
00670 cv::Point2f delta( (float)((A12*b2 - A22*b1) * D),
00671 0);
00672
00673
00674 nextPt += delta;
00675 nextPts[ptidx] = nextPt + halfWin;
00676
00677 if( delta.ddot(delta) <= criteria.epsilon )
00678 break;
00679
00680 if( j > 0 && std::abs(delta.x + prevDelta.x) < 0.01 &&
00681 std::abs(delta.y + prevDelta.y) < 0.01 )
00682 {
00683 nextPts[ptidx] -= delta*0.5f;
00684 break;
00685 }
00686 prevDelta = delta;
00687 }
00688
00689 if( status[ptidx] && err && level == 0 && (flags & cv::OPTFLOW_LK_GET_MIN_EIGENVALS) == 0 )
00690 {
00691 cv::Point2f nextPoint = nextPts[ptidx] - halfWin;
00692 cv::Point inextPoint;
00693
00694 inextPoint.x = cvFloor(nextPoint.x);
00695 inextPoint.y = cvFloor(nextPoint.y);
00696
00697 if( inextPoint.x < -winSize.width || inextPoint.x >= J.cols ||
00698 inextPoint.y < -winSize.height || inextPoint.y >= J.rows )
00699 {
00700 if( status )
00701 status[ptidx] = false;
00702 continue;
00703 }
00704
00705 float aa = nextPoint.x - inextPoint.x;
00706 float bb = nextPoint.y - inextPoint.y;
00707 iw00 = cvRound((1.f - aa)*(1.f - bb)*(1 << W_BITS));
00708 iw01 = cvRound(aa*(1.f - bb)*(1 << W_BITS));
00709 iw10 = cvRound((1.f - aa)*bb*(1 << W_BITS));
00710 iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
00711 float errval = 0.f;
00712
00713 for( y = 0; y < winSize.height; y++ )
00714 {
00715 const uchar* Jptr = J.ptr() + (y + inextPoint.y)*stepJ + inextPoint.x*cn;
00716 const short* Iptr = IWinBuf.ptr<short>(y);
00717
00718 for( x = 0; x < winSize.width*cn; x++ )
00719 {
00720 int diff = CV_DESCALE(Jptr[x]*iw00 + Jptr[x+cn]*iw01 +
00721 Jptr[x+stepJ]*iw10 + Jptr[x+stepJ+cn]*iw11,
00722 W_BITS1-5) - Iptr[x];
00723 errval += std::abs((float)diff);
00724 }
00725 }
00726 err[ptidx] = errval * 1.f/(32*winSize.width*cn*winSize.height);
00727 }
00728 }
00729 }
00730
00731 }
00732 }
00733
00734 cv::Mat disparityFromStereoImages(
00735 const cv::Mat & leftImage,
00736 const cv::Mat & rightImage,
00737 const ParametersMap & parameters)
00738 {
00739 UASSERT(!leftImage.empty() && !rightImage.empty());
00740 UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
00741 UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1);
00742
00743 cv::Mat leftMono;
00744 if(leftImage.channels() == 3)
00745 {
00746 cv::cvtColor(leftImage, leftMono, CV_BGR2GRAY);
00747 }
00748 else
00749 {
00750 leftMono = leftImage;
00751 }
00752 cv::Mat disparity;
00753
00754 StereoBM stereo(parameters);
00755 return stereo.computeDisparity(leftMono, rightImage);
00756 }
00757
00758 cv::Mat depthFromDisparity(const cv::Mat & disparity,
00759 float fx, float baseline,
00760 int type)
00761 {
00762 UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
00763 UASSERT(type == CV_32FC1 || type == CV_16UC1);
00764 cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type);
00765 int countOverMax = 0;
00766 for (int i = 0; i < disparity.rows; i++)
00767 {
00768 for (int j = 0; j < disparity.cols; j++)
00769 {
00770 float disparity_value = disparity.type() == CV_16SC1?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
00771 if (disparity_value > 0.0f)
00772 {
00773
00774 float d = baseline * fx / disparity_value;
00775 if(d>0)
00776 {
00777 if(depth.type() == CV_32FC1)
00778 {
00779 depth.at<float>(i,j) = d;
00780 }
00781 else
00782 {
00783 if(d*1000.0f <= (float)USHRT_MAX)
00784 {
00785 depth.at<unsigned short>(i,j) = (unsigned short)(d*1000.0f);
00786 }
00787 else
00788 {
00789 ++countOverMax;
00790 }
00791 }
00792 }
00793 }
00794 }
00795 }
00796 if(countOverMax)
00797 {
00798 UWARN("Depth conversion error, %d depth values ignored because they are over the maximum depth allowed (65535 mm).", countOverMax);
00799 }
00800 return depth;
00801 }
00802
00803 cv::Mat depthFromStereoImages(
00804 const cv::Mat & leftImage,
00805 const cv::Mat & rightImage,
00806 const std::vector<cv::Point2f> & leftCorners,
00807 float fx,
00808 float baseline,
00809 int flowWinSize,
00810 int flowMaxLevel,
00811 int flowIterations,
00812 double flowEps)
00813 {
00814 UASSERT(!leftImage.empty() && !rightImage.empty() &&
00815 leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
00816 leftImage.cols == rightImage.cols &&
00817 leftImage.rows == rightImage.rows);
00818 UASSERT(fx > 0.0f && baseline > 0.0f);
00819
00820
00821 std::vector<unsigned char> status;
00822 std::vector<float> err;
00823 std::vector<cv::Point2f> rightCorners;
00824 UDEBUG("cv::calcOpticalFlowPyrLK() begin");
00825 cv::calcOpticalFlowPyrLK(
00826 leftImage,
00827 rightImage,
00828 leftCorners,
00829 rightCorners,
00830 status,
00831 err,
00832 cv::Size(flowWinSize, flowWinSize), flowMaxLevel,
00833 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations, flowEps),
00834 cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
00835 UDEBUG("cv::calcOpticalFlowPyrLK() end");
00836
00837 return depthFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, fx, baseline);
00838 }
00839
00840 cv::Mat disparityFromStereoCorrespondences(
00841 const cv::Size & disparitySize,
00842 const std::vector<cv::Point2f> & leftCorners,
00843 const std::vector<cv::Point2f> & rightCorners,
00844 const std::vector<unsigned char> & mask)
00845 {
00846 UASSERT(leftCorners.size() == rightCorners.size());
00847 UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
00848 cv::Mat disparity = cv::Mat::zeros(disparitySize, CV_32FC1);
00849 for(unsigned int i=0; i<leftCorners.size(); ++i)
00850 {
00851 if(mask.empty() || mask[i])
00852 {
00853 cv::Point2i dispPt(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f));
00854 UASSERT(dispPt.x >= 0 && dispPt.x < disparitySize.width);
00855 UASSERT(dispPt.y >= 0 && dispPt.y < disparitySize.height);
00856 disparity.at<float>(dispPt.y, dispPt.x) = leftCorners[i].x - rightCorners[i].x;
00857 }
00858 }
00859 return disparity;
00860 }
00861
00862 cv::Mat depthFromStereoCorrespondences(
00863 const cv::Mat & leftImage,
00864 const std::vector<cv::Point2f> & leftCorners,
00865 const std::vector<cv::Point2f> & rightCorners,
00866 const std::vector<unsigned char> & mask,
00867 float fx, float baseline)
00868 {
00869 UASSERT(!leftImage.empty() && leftCorners.size() == rightCorners.size());
00870 UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
00871 cv::Mat depth = cv::Mat::zeros(leftImage.rows, leftImage.cols, CV_32FC1);
00872 for(unsigned int i=0; i<leftCorners.size(); ++i)
00873 {
00874 if(mask.size() == 0 || mask[i])
00875 {
00876 float disparity = leftCorners[i].x - rightCorners[i].x;
00877 if(disparity > 0.0f)
00878 {
00879 float d = baseline * fx / disparity;
00880 depth.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
00881 }
00882 }
00883 }
00884 return depth;
00885 }
00886
00887 cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
00888 {
00889 UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
00890 cv::Mat depth16U;
00891 if(!depth32F.empty())
00892 {
00893 depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
00894 int countOverMax = 0;
00895 for(int i=0; i<depth32F.rows; ++i)
00896 {
00897 for(int j=0; j<depth32F.cols; ++j)
00898 {
00899 float depth = (depth32F.at<float>(i,j)*1000.0f);
00900 unsigned short depthMM = 0;
00901 if(depth > 0 && depth <= (float)USHRT_MAX)
00902 {
00903 depthMM = (unsigned short)depth;
00904 }
00905 else if(depth > (float)USHRT_MAX)
00906 {
00907 ++countOverMax;
00908 }
00909 depth16U.at<unsigned short>(i, j) = depthMM;
00910 }
00911 }
00912 if(countOverMax)
00913 {
00914 UWARN("Depth conversion error, %d depth values ignored because "
00915 "they are over the maximum depth allowed (65535 mm). Is the depth "
00916 "image really in meters? 32 bits images should be in meters, "
00917 "and 16 bits should be in mm.", countOverMax);
00918 }
00919 }
00920 return depth16U;
00921 }
00922
00923 cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
00924 {
00925 UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
00926 cv::Mat depth32F;
00927 if(!depth16U.empty())
00928 {
00929 depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
00930 for(int i=0; i<depth16U.rows; ++i)
00931 {
00932 for(int j=0; j<depth16U.cols; ++j)
00933 {
00934 float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
00935 depth32F.at<float>(i, j) = depth;
00936 }
00937 }
00938 }
00939 return depth32F;
00940 }
00941
00942 float getDepth(
00943 const cv::Mat & depthImage,
00944 float x, float y,
00945 bool smoothing,
00946 float depthErrorRatio,
00947 bool estWithNeighborsIfNull)
00948 {
00949 UASSERT(!depthImage.empty());
00950 UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
00951
00952 int u = int(x+0.5f);
00953 int v = int(y+0.5f);
00954 if(u == depthImage.cols && x<float(depthImage.cols))
00955 {
00956 u = depthImage.cols - 1;
00957 }
00958 if(v == depthImage.rows && y<float(depthImage.rows))
00959 {
00960 v = depthImage.rows - 1;
00961 }
00962
00963 if(!(u >=0 && u<depthImage.cols && v >=0 && v<depthImage.rows))
00964 {
00965 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)",
00966 x,u,y,v,depthImage.cols, depthImage.rows);
00967 return 0;
00968 }
00969
00970 bool isInMM = depthImage.type() == CV_16UC1;
00971
00972
00973
00974
00975
00976
00977
00978 int u_start = std::max(u-1, 0);
00979 int v_start = std::max(v-1, 0);
00980 int u_end = std::min(u+1, depthImage.cols-1);
00981 int v_end = std::min(v+1, depthImage.rows-1);
00982
00983 float depth = 0.0f;
00984 if(isInMM)
00985 {
00986 if(depthImage.at<unsigned short>(v,u) > 0 &&
00987 depthImage.at<unsigned short>(v,u) < std::numeric_limits<unsigned short>::max())
00988 {
00989 depth = float(depthImage.at<unsigned short>(v,u))*0.001f;
00990 }
00991 }
00992 else
00993 {
00994 depth = depthImage.at<float>(v,u);
00995 }
00996
00997 if((depth==0.0f || !uIsFinite(depth)) && estWithNeighborsIfNull)
00998 {
00999
01000 float tmp = 0.0f;
01001 int count = 0;
01002 for(int uu = u_start; uu <= u_end; ++uu)
01003 {
01004 for(int vv = v_start; vv <= v_end; ++vv)
01005 {
01006 if((uu == u && vv!=v) || (uu != u && vv==v))
01007 {
01008 float d = 0.0f;
01009 if(isInMM)
01010 {
01011 if(depthImage.at<unsigned short>(vv,uu) > 0 &&
01012 depthImage.at<unsigned short>(vv,uu) < std::numeric_limits<unsigned short>::max())
01013 {
01014 d = float(depthImage.at<unsigned short>(vv,uu))*0.001f;
01015 }
01016 }
01017 else
01018 {
01019 d = depthImage.at<float>(vv,uu);
01020 }
01021 if(d!=0.0f && uIsFinite(d))
01022 {
01023 if(tmp == 0.0f)
01024 {
01025 tmp = d;
01026 ++count;
01027 }
01028 else
01029 {
01030 float depthError = depthErrorRatio * tmp;
01031 if(fabs(d - tmp/float(count)) < depthError)
01032
01033 {
01034 tmp += d;
01035 ++count;
01036 }
01037 }
01038 }
01039 }
01040 }
01041 }
01042 if(count > 1)
01043 {
01044 depth = tmp/float(count);
01045 }
01046 }
01047
01048 if(depth!=0.0f && uIsFinite(depth))
01049 {
01050 if(smoothing)
01051 {
01052 float sumWeights = 0.0f;
01053 float sumDepths = 0.0f;
01054 for(int uu = u_start; uu <= u_end; ++uu)
01055 {
01056 for(int vv = v_start; vv <= v_end; ++vv)
01057 {
01058 if(!(uu == u && vv == v))
01059 {
01060 float d = 0.0f;
01061 if(isInMM)
01062 {
01063 if(depthImage.at<unsigned short>(vv,uu) > 0 &&
01064 depthImage.at<unsigned short>(vv,uu) < std::numeric_limits<unsigned short>::max())
01065 {
01066 d = float(depthImage.at<unsigned short>(vv,uu))*0.001f;
01067 }
01068 }
01069 else
01070 {
01071 d = depthImage.at<float>(vv,uu);
01072 }
01073
01074 float depthError = depthErrorRatio * depth;
01075
01076
01077 if(d != 0.0f && uIsFinite(d) && fabs(d - depth) < depthError)
01078 {
01079 if(uu == u || vv == v)
01080 {
01081 sumWeights+=2.0f;
01082 d*=2.0f;
01083 }
01084 else
01085 {
01086 sumWeights+=1.0f;
01087 }
01088 sumDepths += d;
01089 }
01090 }
01091 }
01092 }
01093
01094 depth *= 4.0f;
01095 sumWeights += 4.0f;
01096
01097
01098 depth = (depth+sumDepths)/sumWeights;
01099 }
01100 }
01101 else
01102 {
01103 depth = 0;
01104 }
01105 return depth;
01106 }
01107
01108 cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios)
01109 {
01110 return computeRoi(image.size(), roiRatios);
01111 }
01112
01113 cv::Rect computeRoi(const cv::Size & imageSize, const std::string & roiRatios)
01114 {
01115 std::list<std::string> strValues = uSplit(roiRatios, ' ');
01116 if(strValues.size() != 4)
01117 {
01118 UERROR("The number of values must be 4 (roi=\"%s\")", roiRatios.c_str());
01119 }
01120 else
01121 {
01122 std::vector<float> values(4);
01123 unsigned int i=0;
01124 for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
01125 {
01126 values[i] = uStr2Float(*iter);
01127 ++i;
01128 }
01129
01130 if(values[0] >= 0 && values[0] < 1 && values[0] < 1.0f-values[1] &&
01131 values[1] >= 0 && values[1] < 1 && values[1] < 1.0f-values[0] &&
01132 values[2] >= 0 && values[2] < 1 && values[2] < 1.0f-values[3] &&
01133 values[3] >= 0 && values[3] < 1 && values[3] < 1.0f-values[2])
01134 {
01135 return computeRoi(imageSize, values);
01136 }
01137 else
01138 {
01139 UERROR("The roi ratios are not valid (roi=\"%s\")", roiRatios.c_str());
01140 }
01141 }
01142 return cv::Rect();
01143 }
01144
01145 cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
01146 {
01147 return computeRoi(image.size(), roiRatios);
01148 }
01149
01150 cv::Rect computeRoi(const cv::Size & imageSize, const std::vector<float> & roiRatios)
01151 {
01152 if(imageSize.height!=0 && imageSize.width!= 0 && roiRatios.size() == 4)
01153 {
01154 float width = imageSize.width;
01155 float height = imageSize.height;
01156 cv::Rect roi(0, 0, width, height);
01157 UDEBUG("roi ratios = %f, %f, %f, %f", roiRatios[0],roiRatios[1],roiRatios[2],roiRatios[3]);
01158 UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
01159
01160
01161 if(roiRatios[0] > 0 && roiRatios[0] < 1.0f - roiRatios[1])
01162 {
01163 roi.x = width * roiRatios[0];
01164 }
01165
01166
01167 if(roiRatios[1] > 0 && roiRatios[1] < 1.0f - roiRatios[0])
01168 {
01169 roi.width -= width * roiRatios[1];
01170 }
01171 roi.width -= roi.x;
01172
01173
01174 if(roiRatios[2] > 0 && roiRatios[2] < 1.0f - roiRatios[3])
01175 {
01176 roi.y = height * roiRatios[2];
01177 }
01178
01179
01180 if(roiRatios[3] > 0 && roiRatios[3] < 1.0f - roiRatios[2])
01181 {
01182 roi.height -= height * roiRatios[3];
01183 }
01184 roi.height -= roi.y;
01185 UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
01186
01187 return roi;
01188 }
01189 else
01190 {
01191 UERROR("Image is null or _roiRatios(=%d) != 4", roiRatios.size());
01192 return cv::Rect();
01193 }
01194 }
01195
01196 cv::Mat decimate(const cv::Mat & image, int decimation)
01197 {
01198 UASSERT(decimation >= 1);
01199 cv::Mat out;
01200 if(!image.empty())
01201 {
01202 if(decimation > 1)
01203 {
01204 if((image.type() == CV_32FC1 || image.type()==CV_16UC1))
01205 {
01206 UASSERT_MSG(image.rows % decimation == 0 && image.cols % decimation == 0,
01207 uFormat("Decimation of depth images should be exact! (decimation=%d, size=%dx%d)",
01208 decimation, image.cols, image.rows).c_str());
01209
01210 out = cv::Mat(image.rows/decimation, image.cols/decimation, image.type());
01211 if(image.type() == CV_32FC1)
01212 {
01213 for(int j=0; j<out.rows; ++j)
01214 {
01215 for(int i=0; i<out.cols; ++i)
01216 {
01217 out.at<float>(j, i) = image.at<float>(j*decimation, i*decimation);
01218 }
01219 }
01220 }
01221 else
01222 {
01223 for(int j=0; j<out.rows; ++j)
01224 {
01225 for(int i=0; i<out.cols; ++i)
01226 {
01227 out.at<unsigned short>(j, i) = image.at<unsigned short>(j*decimation, i*decimation);
01228 }
01229 }
01230 }
01231 }
01232 else
01233 {
01234 cv::resize(image, out, cv::Size(), 1.0f/float(decimation), 1.0f/float(decimation), cv::INTER_AREA);
01235 }
01236 }
01237 else
01238 {
01239 out = image;
01240 }
01241 }
01242 return out;
01243 }
01244
01245 cv::Mat interpolate(const cv::Mat & image, int factor, float depthErrorRatio)
01246 {
01247 UASSERT_MSG(factor >= 1, uFormat("factor=%d", factor).c_str());
01248 cv::Mat out;
01249 if(!image.empty())
01250 {
01251 if(factor > 1)
01252 {
01253 if((image.type() == CV_32FC1 || image.type()==CV_16UC1))
01254 {
01255 UASSERT(depthErrorRatio>0.0f);
01256 out = cv::Mat::zeros(image.rows*factor, image.cols*factor, image.type());
01257 for(int j=0; j<out.rows; j+=factor)
01258 {
01259 for(int i=0; i<out.cols; i+=factor)
01260 {
01261 if(i>0 && j>0)
01262 {
01263 float dTopLeft;
01264 float dTopRight;
01265 float dBottomLeft;
01266 float dBottomRight;
01267 if(image.type() == CV_32FC1)
01268 {
01269 dTopLeft = image.at<float>(j/factor-1, i/factor-1);
01270 dTopRight = image.at<float>(j/factor-1, i/factor);
01271 dBottomLeft = image.at<float>(j/factor, i/factor-1);
01272 dBottomRight = image.at<float>(j/factor, i/factor);
01273 }
01274 else
01275 {
01276 dTopLeft = image.at<unsigned short>(j/factor-1, i/factor-1);
01277 dTopRight = image.at<unsigned short>(j/factor-1, i/factor);
01278 dBottomLeft = image.at<unsigned short>(j/factor, i/factor-1);
01279 dBottomRight = image.at<unsigned short>(j/factor, i/factor);
01280 }
01281
01282 if(dTopLeft>0 && dTopRight>0 && dBottomLeft>0 && dBottomRight > 0)
01283 {
01284 float depthError = depthErrorRatio*(dTopLeft+dTopRight+dBottomLeft+dBottomRight)/4.0f;
01285 if(fabs(dTopLeft-dTopRight) <= depthError &&
01286 fabs(dTopLeft-dBottomLeft) <= depthError &&
01287 fabs(dTopLeft-dBottomRight) <= depthError)
01288 {
01289
01290
01291 float slopeTop = (dTopRight-dTopLeft)/float(factor);
01292 float slopeBottom = (dBottomRight-dBottomLeft)/float(factor);
01293 if(image.type() == CV_32FC1)
01294 {
01295 for(int z=i-factor; z<=i; ++z)
01296 {
01297 out.at<float>(j-factor, z) = dTopLeft+(slopeTop*float(z-(i-factor)));
01298 out.at<float>(j, z) = dBottomLeft+(slopeBottom*float(z-(i-factor)));
01299 }
01300 }
01301 else
01302 {
01303 for(int z=i-factor; z<=i; ++z)
01304 {
01305 out.at<unsigned short>(j-factor, z) = (unsigned short)(dTopLeft+(slopeTop*float(z-(i-factor))));
01306 out.at<unsigned short>(j, z) = (unsigned short)(dBottomLeft+(slopeBottom*float(z-(i-factor))));
01307 }
01308 }
01309
01310
01311 if(image.type() == CV_32FC1)
01312 {
01313 for(int z=i-factor; z<=i; ++z)
01314 {
01315 float top = out.at<float>(j-factor, z);
01316 float bottom = out.at<float>(j, z);
01317 float slope = (bottom-top)/float(factor);
01318 for(int d=j-factor+1; d<j; ++d)
01319 {
01320 out.at<float>(d, z) = top+(slope*float(d-(j-factor)));
01321 }
01322 }
01323 }
01324 else
01325 {
01326 for(int z=i-factor; z<=i; ++z)
01327 {
01328 float top = out.at<unsigned short>(j-factor, z);
01329 float bottom = out.at<unsigned short>(j, z);
01330 float slope = (bottom-top)/float(factor);
01331 for(int d=j-factor+1; d<j; ++d)
01332 {
01333 out.at<unsigned short>(d, z) = (unsigned short)(top+(slope*float(d-(j-factor))));
01334 }
01335 }
01336 }
01337 }
01338 }
01339 }
01340 }
01341 }
01342 }
01343 else
01344 {
01345 cv::resize(image, out, cv::Size(), float(factor), float(factor));
01346 }
01347 }
01348 else
01349 {
01350 out = image;
01351 }
01352 }
01353 return out;
01354 }
01355
01356
01357 cv::Mat registerDepth(
01358 const cv::Mat & depth,
01359 const cv::Mat & depthK,
01360 const cv::Size & colorSize,
01361 const cv::Mat & colorK,
01362 const rtabmap::Transform & transform)
01363 {
01364 UASSERT(!transform.isNull());
01365 UASSERT(!depth.empty());
01366 UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
01367 UASSERT(depthK.type() == CV_64FC1 && depthK.cols == 3 && depthK.cols == 3);
01368 UASSERT(colorK.type() == CV_64FC1 && colorK.cols == 3 && colorK.cols == 3);
01369
01370 float fx = depthK.at<double>(0,0);
01371 float fy = depthK.at<double>(1,1);
01372 float cx = depthK.at<double>(0,2);
01373 float cy = depthK.at<double>(1,2);
01374
01375 float rfx = colorK.at<double>(0,0);
01376 float rfy = colorK.at<double>(1,1);
01377 float rcx = colorK.at<double>(0,2);
01378 float rcy = colorK.at<double>(1,2);
01379
01380
01381
01382
01383 Eigen::Affine3f proj = transform.toEigen3f();
01384 Eigen::Vector4f P4,P3;
01385 P4[3] = 1;
01386 cv::Mat registered = cv::Mat::zeros(colorSize, depth.type());
01387
01388 bool depthInMM = depth.type() == CV_16UC1;
01389 for(int y=0; y<depth.rows; ++y)
01390 {
01391 for(int x=0; x<depth.cols; ++x)
01392 {
01393
01394 float dz = depthInMM?float(depth.at<unsigned short>(y,x))*0.001f:depth.at<float>(y,x);
01395 if(dz>=0.0f)
01396 {
01397
01398 P4[0] = (x - cx) * dz / fx;
01399 P4[1] = (y - cy) * dz / fy;
01400 P4[2] = dz;
01401
01402 P3 = proj * P4;
01403 float z = P3[2];
01404 float invZ = 1.0f/z;
01405 int dx = (rfx*P3[0])*invZ + rcx;
01406 int dy = (rfy*P3[1])*invZ + rcy;
01407
01408 if(uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
01409 {
01410 if(depthInMM)
01411 {
01412 unsigned short z16 = z * 1000;
01413 unsigned short &zReg = registered.at<unsigned short>(dy, dx);
01414 if(zReg == 0 || z16 < zReg)
01415 {
01416 zReg = z16;
01417 }
01418 }
01419 else
01420 {
01421 float &zReg = registered.at<float>(dy, dx);
01422 if(zReg == 0 || z < zReg)
01423 {
01424 zReg = z;
01425 }
01426 }
01427 }
01428 }
01429 }
01430 }
01431 return registered;
01432 }
01433
01434 cv::Mat fillDepthHoles(const cv::Mat & depth, int maximumHoleSize, float errorRatio)
01435 {
01436 UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
01437 UASSERT(maximumHoleSize > 0);
01438 cv::Mat output = depth.clone();
01439 bool isMM = depth.type() == CV_16UC1;
01440 for(int y=0; y<depth.rows-2; ++y)
01441 {
01442 for(int x=0; x<depth.cols-2; ++x)
01443 {
01444 float a, bRight, bDown;
01445 if(isMM)
01446 {
01447 a = depth.at<unsigned short>(y, x);
01448 bRight = depth.at<unsigned short>(y, x+1);
01449 bDown = depth.at<unsigned short>(y+1, x);
01450 }
01451 else
01452 {
01453 a = depth.at<float>(y, x);
01454 bRight = depth.at<float>(y, x+1);
01455 bDown = depth.at<float>(y+1, x);
01456 }
01457
01458 if(a > 0.0f && (bRight == 0.0f || bDown == 0.0f))
01459 {
01460 bool horizontalSet = bRight != 0.0f;
01461 bool verticalSet = bDown != 0.0f;
01462 int stepX = 0;
01463 for(int h=1; h<=maximumHoleSize && (!horizontalSet || !verticalSet); ++h)
01464 {
01465
01466 if(!horizontalSet)
01467 {
01468 if(x+1+h >= depth.cols)
01469 {
01470 horizontalSet = true;
01471 }
01472 else
01473 {
01474 float c = isMM?depth.at<unsigned short>(y, x+1+h):depth.at<float>(y, x+1+h);
01475 if(c == 0)
01476 {
01477
01478 }
01479 else
01480 {
01481
01482 float depthError = errorRatio*float(a+c)/2.0f;
01483 if(fabs(a-c) <= depthError)
01484 {
01485
01486 float slope = (c-a)/float(h+1);
01487 if(isMM)
01488 {
01489 for(int z=x+1; z<x+1+h; ++z)
01490 {
01491 unsigned short & value = output.at<unsigned short>(y, z);
01492 if(value == 0)
01493 {
01494 value = (unsigned short)(a+(slope*float(z-x)));
01495 }
01496 else
01497 {
01498
01499 value = (value+(unsigned short)(a+(slope*float(z-x))))/2;
01500 }
01501 }
01502 }
01503 else
01504 {
01505 for(int z=x+1; z<x+1+h; ++z)
01506 {
01507 float & value = output.at<float>(y, z);
01508 if(value == 0)
01509 {
01510 value = a+(slope*float(z-x));
01511 }
01512 else
01513 {
01514
01515 value = (value+(a+(slope*float(z-x))))/2;
01516 }
01517 }
01518 }
01519 }
01520 horizontalSet = true;
01521 stepX = h;
01522 }
01523 }
01524 }
01525
01526
01527 if(!verticalSet)
01528 {
01529 if(y+1+h >= depth.rows)
01530 {
01531 verticalSet = true;
01532 }
01533 else
01534 {
01535 float c = isMM?depth.at<unsigned short>(y+1+h, x):depth.at<float>(y+1+h, x);
01536 if(c == 0)
01537 {
01538
01539 }
01540 else
01541 {
01542
01543 float depthError = errorRatio*float(a+c)/2.0f;
01544 if(fabs(a-c) <= depthError)
01545 {
01546
01547 float slope = (c-a)/float(h+1);
01548 if(isMM)
01549 {
01550 for(int z=y+1; z<y+1+h; ++z)
01551 {
01552 unsigned short & value = output.at<unsigned short>(z, x);
01553 if(value == 0)
01554 {
01555 value = (unsigned short)(a+(slope*float(z-y)));
01556 }
01557 else
01558 {
01559
01560 value = (value+(unsigned short)(a+(slope*float(z-y))))/2;
01561 }
01562 }
01563 }
01564 else
01565 {
01566 for(int z=y+1; z<y+1+h; ++z)
01567 {
01568 float & value = output.at<float>(z, x);
01569 if(value == 0)
01570 {
01571 value = (a+(slope*float(z-y)));
01572 }
01573 else
01574 {
01575
01576 value = (value+(a+(slope*float(z-y))))/2;
01577 }
01578 }
01579 }
01580 }
01581 verticalSet = true;
01582 }
01583 }
01584 }
01585 }
01586 x+=stepX;
01587 }
01588 }
01589 }
01590 return output;
01591 }
01592
01593 void fillRegisteredDepthHoles(cv::Mat & registeredDepth, bool vertical, bool horizontal, bool fillDoubleHoles)
01594 {
01595 UASSERT(registeredDepth.type() == CV_16UC1);
01596 int margin = fillDoubleHoles?2:1;
01597 for(int x=1; x<registeredDepth.cols-margin; ++x)
01598 {
01599 for(int y=1; y<registeredDepth.rows-margin; ++y)
01600 {
01601 unsigned short & b = registeredDepth.at<unsigned short>(y, x);
01602 bool set = false;
01603 if(vertical)
01604 {
01605 const unsigned short & a = registeredDepth.at<unsigned short>(y-1, x);
01606 unsigned short & c = registeredDepth.at<unsigned short>(y+1, x);
01607 if(a && c)
01608 {
01609 unsigned short error = 0.01*((a+c)/2);
01610 if(((b == 0 && a && c) || (b > a+error && b > c+error)) &&
01611 (a>c?a-c<=error:c-a<=error))
01612 {
01613 b = (a+c)/2;
01614 set = true;
01615 if(!horizontal)
01616 {
01617 ++y;
01618 }
01619 }
01620 }
01621 if(!set && fillDoubleHoles)
01622 {
01623 const unsigned short & d = registeredDepth.at<unsigned short>(y+2, x);
01624 if(a && d && (b==0 || c==0))
01625 {
01626 unsigned short error = 0.01*((a+d)/2);
01627 if(((b == 0 && a && d) || (b > a+error && b > d+error)) &&
01628 ((c == 0 && a && d) || (c > a+error && c > d+error)) &&
01629 (a>d?a-d<=error:d-a<=error))
01630 {
01631 if(a>d)
01632 {
01633 unsigned short tmp = (a-d)/4;
01634 b = d + tmp;
01635 c = d + 3*tmp;
01636 }
01637 else
01638 {
01639 unsigned short tmp = (d-a)/4;
01640 b = a + tmp;
01641 c = a + 3*tmp;
01642 }
01643 set = true;
01644 if(!horizontal)
01645 {
01646 y+=2;
01647 }
01648 }
01649 }
01650 }
01651 }
01652 if(!set && horizontal)
01653 {
01654 const unsigned short & a = registeredDepth.at<unsigned short>(y, x-1);
01655 unsigned short & c = registeredDepth.at<unsigned short>(y, x+1);
01656 if(a && c)
01657 {
01658 unsigned short error = 0.01*((a+c)/2);
01659 if(((b == 0 && a && c) || (b > a+error && b > c+error)) &&
01660 (a>c?a-c<=error:c-a<=error))
01661 {
01662 b = (a+c)/2;
01663 set = true;
01664 }
01665 }
01666 if(!set && fillDoubleHoles)
01667 {
01668 const unsigned short & d = registeredDepth.at<unsigned short>(y, x+2);
01669 if(a && d && (b==0 || c==0))
01670 {
01671 unsigned short error = 0.01*((a+d)/2);
01672 if(((b == 0 && a && d) || (b > a+error && b > d+error)) &&
01673 ((c == 0 && a && d) || (c > a+error && c > d+error)) &&
01674 (a>d?a-d<=error:d-a<=error))
01675 {
01676 if(a>d)
01677 {
01678 unsigned short tmp = (a-d)/4;
01679 b = d + tmp;
01680 c = d + 3*tmp;
01681 }
01682 else
01683 {
01684 unsigned short tmp = (d-a)/4;
01685 b = a + tmp;
01686 c = a + 3*tmp;
01687 }
01688 }
01689 }
01690 }
01691 }
01692 }
01693 }
01694 }
01695
01696
01697 class Array3D
01698 {
01699 public:
01700 Array3D (const size_t width, const size_t height, const size_t depth)
01701 {
01702 x_dim_ = width;
01703 y_dim_ = height;
01704 z_dim_ = depth;
01705 v_ = std::vector<Eigen::Vector2f> (width*height*depth, Eigen::Vector2f (0.0f, 0.0f));
01706 }
01707
01708 inline Eigen::Vector2f&
01709 operator () (const size_t x, const size_t y, const size_t z)
01710 { return v_[(x * y_dim_ + y) * z_dim_ + z]; }
01711
01712 inline const Eigen::Vector2f&
01713 operator () (const size_t x, const size_t y, const size_t z) const
01714 { return v_[(x * y_dim_ + y) * z_dim_ + z]; }
01715
01716 inline void
01717 resize (const size_t width, const size_t height, const size_t depth)
01718 {
01719 x_dim_ = width;
01720 y_dim_ = height;
01721 z_dim_ = depth;
01722 v_.resize (x_dim_ * y_dim_ * z_dim_);
01723 }
01724
01725 Eigen::Vector2f
01726 trilinear_interpolation (const float x,
01727 const float y,
01728 const float z)
01729 {
01730 const size_t x_index = clamp (0, x_dim_ - 1, static_cast<size_t> (x));
01731 const size_t xx_index = clamp (0, x_dim_ - 1, x_index + 1);
01732
01733 const size_t y_index = clamp (0, y_dim_ - 1, static_cast<size_t> (y));
01734 const size_t yy_index = clamp (0, y_dim_ - 1, y_index + 1);
01735
01736 const size_t z_index = clamp (0, z_dim_ - 1, static_cast<size_t> (z));
01737 const size_t zz_index = clamp (0, z_dim_ - 1, z_index + 1);
01738
01739 const float x_alpha = x - static_cast<float> (x_index);
01740 const float y_alpha = y - static_cast<float> (y_index);
01741 const float z_alpha = z - static_cast<float> (z_index);
01742
01743 return
01744 (1.0f-x_alpha) * (1.0f-y_alpha) * (1.0f-z_alpha) * (*this)(x_index, y_index, z_index) +
01745 x_alpha * (1.0f-y_alpha) * (1.0f-z_alpha) * (*this)(xx_index, y_index, z_index) +
01746 (1.0f-x_alpha) * y_alpha * (1.0f-z_alpha) * (*this)(x_index, yy_index, z_index) +
01747 x_alpha * y_alpha * (1.0f-z_alpha) * (*this)(xx_index, yy_index, z_index) +
01748 (1.0f-x_alpha) * (1.0f-y_alpha) * z_alpha * (*this)(x_index, y_index, zz_index) +
01749 x_alpha * (1.0f-y_alpha) * z_alpha * (*this)(xx_index, y_index, zz_index) +
01750 (1.0f-x_alpha) * y_alpha * z_alpha * (*this)(x_index, yy_index, zz_index) +
01751 x_alpha * y_alpha * z_alpha * (*this)(xx_index, yy_index, zz_index);
01752 }
01753
01754 static inline size_t
01755 clamp (const size_t min_value,
01756 const size_t max_value,
01757 const size_t x)
01758 {
01759 if (x >= min_value && x <= max_value)
01760 {
01761 return x;
01762 }
01763 else if (x < min_value)
01764 {
01765 return (min_value);
01766 }
01767 else
01768 {
01769 return (max_value);
01770 }
01771 }
01772
01773 inline size_t
01774 x_size () const
01775 { return x_dim_; }
01776
01777 inline size_t
01778 y_size () const
01779 { return y_dim_; }
01780
01781 inline size_t
01782 z_size () const
01783 { return z_dim_; }
01784
01785 inline std::vector<Eigen::Vector2f >::iterator
01786 begin ()
01787 { return v_.begin (); }
01788
01789 inline std::vector<Eigen::Vector2f >::iterator
01790 end ()
01791 { return v_.end (); }
01792
01793 inline std::vector<Eigen::Vector2f >::const_iterator
01794 begin () const
01795 { return v_.begin (); }
01796
01797 inline std::vector<Eigen::Vector2f >::const_iterator
01798 end () const
01799 { return v_.end (); }
01800
01801 private:
01802 std::vector<Eigen::Vector2f > v_;
01803 size_t x_dim_, y_dim_, z_dim_;
01804 };
01805
01809 cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR, bool earlyDivision)
01810 {
01811 UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1));
01812 UDEBUG("Begin: depth float=%d %dx%d sigmaS=%f sigmaR=%f earlDivision=%d",
01813 depth.type()==CV_32FC1?1:0, depth.cols, depth.rows, sigmaS, sigmaR, earlyDivision?1:0);
01814
01815 cv::Mat output = cv::Mat::zeros(depth.size(), CV_32FC1);
01816
01817 float base_max = -std::numeric_limits<float>::max ();
01818 float base_min = std::numeric_limits<float>::max ();
01819 bool found_finite = false;
01820 for (int x = 0; x < depth.cols; ++x)
01821 for (int y = 0; y < depth.rows; ++y)
01822 {
01823 float z = depth.type()==CV_32FC1?depth.at<float>(y, x):float(depth.at<unsigned short>(y, x))/1000.0f;
01824 if (z > 0.0f && uIsFinite(z))
01825 {
01826 if (base_max < z)
01827 base_max = z;
01828 if (base_min > z)
01829 base_min = z;
01830 found_finite = true;
01831 }
01832 }
01833 if (!found_finite)
01834 {
01835 UWARN("Given an empty depth image. Doing nothing.");
01836 return cv::Mat();
01837 }
01838 UDEBUG("base_min=%f base_max=%f", base_min, base_max);
01839
01840 const float base_delta = base_max - base_min;
01841
01842 const size_t padding_xy = 2;
01843 const size_t padding_z = 2;
01844
01845 const size_t small_width = static_cast<size_t> (static_cast<float> (depth.cols - 1) / sigmaS) + 1 + 2 * padding_xy;
01846 const size_t small_height = static_cast<size_t> (static_cast<float> (depth.rows - 1) / sigmaS) + 1 + 2 * padding_xy;
01847 const size_t small_depth = static_cast<size_t> (base_delta / sigmaR) + 1 + 2 * padding_z;
01848
01849 UDEBUG("small_width=%d small_height=%d small_depth=%d", (int)small_width, (int)small_height, (int)small_depth);
01850 Array3D data (small_width, small_height, small_depth);
01851 for (int x = 0; x < depth.cols; ++x)
01852 {
01853 const size_t small_x = static_cast<size_t> (static_cast<float> (x) / sigmaS + 0.5f) + padding_xy;
01854 for (int y = 0; y < depth.rows; ++y)
01855 {
01856 float v = depth.type()==CV_32FC1?depth.at<float>(y,x):float(depth.at<unsigned short>(y,x))/1000.0f;
01857 if((v > 0 && uIsFinite(v)))
01858 {
01859 float z = v - base_min;
01860
01861 const size_t small_y = static_cast<size_t> (static_cast<float> (y) / sigmaS + 0.5f) + padding_xy;
01862 const size_t small_z = static_cast<size_t> (static_cast<float> (z) / sigmaR + 0.5f) + padding_z;
01863
01864 Eigen::Vector2f& d = data (small_x, small_y, small_z);
01865 d[0] += v;
01866 d[1] += 1.0f;
01867 }
01868 }
01869 }
01870
01871 std::vector<long int> offset (3);
01872 offset[0] = &(data (1,0,0)) - &(data (0,0,0));
01873 offset[1] = &(data (0,1,0)) - &(data (0,0,0));
01874 offset[2] = &(data (0,0,1)) - &(data (0,0,0));
01875
01876 Array3D buffer (small_width, small_height, small_depth);
01877
01878 for (size_t dim = 0; dim < 3; ++dim)
01879 {
01880 const long int off = offset[dim];
01881 for (size_t n_iter = 0; n_iter < 2; ++n_iter)
01882 {
01883 std::swap (buffer, data);
01884 for(size_t x = 1; x < small_width - 1; ++x)
01885 for(size_t y = 1; y < small_height - 1; ++y)
01886 {
01887 Eigen::Vector2f* d_ptr = &(data (x,y,1));
01888 Eigen::Vector2f* b_ptr = &(buffer (x,y,1));
01889
01890 for(size_t z = 1; z < small_depth - 1; ++z, ++d_ptr, ++b_ptr)
01891 *d_ptr = (*(b_ptr - off) + *(b_ptr + off) + 2.0 * (*b_ptr)) / 4.0;
01892 }
01893 }
01894 }
01895
01896 if (earlyDivision)
01897 {
01898 for (std::vector<Eigen::Vector2f>::iterator d = data.begin (); d != data.end (); ++d)
01899 *d /= ((*d)[0] != 0) ? (*d)[1] : 1;
01900 }
01901
01902 for (int x = 0; x < depth.cols; ++x)
01903 for (int y = 0; y < depth.rows; ++y)
01904 {
01905 float z = depth.type()==CV_32FC1?depth.at<float>(y,x):float(depth.at<unsigned short>(y,x))/1000.0f;
01906 if(z > 0 && uIsFinite(z))
01907 {
01908 z -= base_min;
01909 const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigmaS + padding_xy,
01910 static_cast<float> (y) / sigmaS + padding_xy,
01911 z / sigmaR + padding_z);
01912 float v = earlyDivision ? D[0] : D[0] / D[1];
01913 if(v < base_min || v >= base_max)
01914 {
01915 v = 0.0f;
01916 }
01917 if(depth.type()==CV_16UC1 && v>65.5350f)
01918 {
01919 v = 65.5350f;
01920 }
01921 output.at<float>(y,x) = v;
01922 }
01923 }
01924
01925 UDEBUG("End");
01926 return output;
01927 }
01928
01937 cv::Mat brightnessAndContrastAuto(const cv::Mat &src, const cv::Mat & mask, float clipLowHistPercent, float clipHighHistPercent)
01938 {
01939
01940 CV_Assert(clipLowHistPercent >= 0 && clipHighHistPercent>=0);
01941 CV_Assert((src.type() == CV_8UC1) || (src.type() == CV_8UC3) || (src.type() == CV_8UC4));
01942
01943 int histSize = 256;
01944 float alpha, beta;
01945 double minGray = 0, maxGray = 0;
01946
01947
01948 cv::Mat gray;
01949 if (src.type() == CV_8UC1) gray = src;
01950 else if (src.type() == CV_8UC3) cvtColor(src, gray, CV_BGR2GRAY);
01951 else if (src.type() == CV_8UC4) cvtColor(src, gray, CV_BGRA2GRAY);
01952 if (clipLowHistPercent == 0 && clipHighHistPercent == 0)
01953 {
01954
01955 cv::minMaxLoc(gray, &minGray, &maxGray, 0, 0, mask);
01956 }
01957 else
01958 {
01959 cv::Mat hist;
01960
01961 float range[] = { 0, 256 };
01962 const float* histRange = { range };
01963 bool uniform = true;
01964 bool accumulate = false;
01965 calcHist(&gray, 1, 0, mask, hist, 1, &histSize, &histRange, uniform, accumulate);
01966
01967
01968 std::vector<float> accumulator(histSize);
01969 accumulator[0] = hist.at<float>(0);
01970 for (int i = 1; i < histSize; i++)
01971 {
01972 accumulator[i] = accumulator[i - 1] + hist.at<float>(i);
01973 }
01974
01975
01976 float max = accumulator.back();
01977 clipLowHistPercent *= (max / 100.0);
01978 clipHighHistPercent *= (max / 100.0);
01979
01980 minGray = 0;
01981 while (accumulator[minGray] < clipLowHistPercent)
01982 minGray++;
01983
01984
01985 maxGray = histSize - 1;
01986 while (accumulator[maxGray] >= (max - clipHighHistPercent))
01987 maxGray--;
01988 }
01989
01990
01991 float inputRange = maxGray - minGray;
01992
01993 alpha = (histSize - 1) / inputRange;
01994 beta = -minGray * alpha;
01995
01996 UINFO("minGray=%f maxGray=%f alpha=%f beta=%f", minGray, maxGray, alpha, beta);
01997
01998 cv::Mat dst;
01999
02000
02001 src.convertTo(dst, -1, alpha, beta);
02002
02003
02004 if (dst.type() == CV_8UC4)
02005 {
02006 int from_to[] = { 3, 3};
02007 cv::mixChannels(&src, 4, &dst,1, from_to, 1);
02008 }
02009 return dst;
02010 }
02011
02012 cv::Mat exposureFusion(const std::vector<cv::Mat> & images)
02013 {
02014 UASSERT(images.size());
02015 cv::Mat fusion;
02016 #if CV_MAJOR_VERSION >= 3
02017 cv::createMergeMertens()->process(images, fusion);
02018 cv::Mat rgb8;
02019 UASSERT(fusion.channels() == 3);
02020 fusion.convertTo(rgb8, CV_8UC3, 255.0);
02021 fusion = rgb8;
02022 #else
02023 UWARN("Exposure fusion is only available when rtabmap is built with OpenCV3.");
02024 if (images.size())
02025 {
02026 fusion = images[0].clone();
02027 }
02028 #endif
02029 return fusion;
02030 }
02031
02032 }
02033
02034 }