util2d.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other cv::Materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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 <map>
00042 
00043 namespace rtabmap
00044 {
00045 
00046 namespace util2d
00047 {
00048 
00049 // SSD: Sum of Squared Differences
00050 float ssd(const cv::Mat & windowLeft, const cv::Mat & windowRight)
00051 {
00052         UASSERT_MSG(windowLeft.type() == CV_8UC1 || windowLeft.type() == CV_32FC1 || windowLeft.type() == CV_16SC2, uFormat("Type=%d", windowLeft.type()).c_str());
00053         UASSERT(windowLeft.type() == windowRight.type());
00054         UASSERT_MSG(windowLeft.rows == windowRight.rows, uFormat("%d vs %d", windowLeft.rows, windowRight.rows).c_str());
00055         UASSERT_MSG(windowLeft.cols == windowRight.cols, uFormat("%d vs %d", windowLeft.cols, windowRight.cols).c_str());
00056 
00057         float score = 0.0f;
00058         for(int v=0; v<windowLeft.rows; ++v)
00059         {
00060                 for(int u=0; u<windowLeft.cols; ++u)
00061                 {
00062                         float s = 0.0f;
00063                         if(windowLeft.type() == CV_8UC1)
00064                         {
00065                                 s = float(windowLeft.at<unsigned char>(v,u))-float(windowRight.at<unsigned char>(v,u));
00066                         }
00067                         else if(windowLeft.type() == CV_32FC1)
00068                         {
00069                                 s = windowLeft.at<float>(v,u)-windowRight.at<float>(v,u);
00070                         }
00071                         else if(windowLeft.type() == CV_16SC2)
00072                         {
00073                                 float sL = float(windowLeft.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowLeft.at<cv::Vec2s>(v,u)[1])*0.5f;
00074                                 float sR = float(windowRight.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowRight.at<cv::Vec2s>(v,u)[1])*0.5f;
00075                                 s = sL - sR;
00076                         }
00077 
00078                         score += s*s;
00079                 }
00080         }
00081         return score;
00082 }
00083 
00084 // SAD: Sum of Absolute intensity Differences
00085 float sad(const cv::Mat & windowLeft, const cv::Mat & windowRight)
00086 {
00087         UASSERT_MSG(windowLeft.type() == CV_8UC1 || windowLeft.type() == CV_32FC1 || windowLeft.type() == CV_16SC2, uFormat("Type=%d", windowLeft.type()).c_str());
00088         UASSERT(windowLeft.type() == windowRight.type());
00089         UASSERT_MSG(windowLeft.rows == windowRight.rows, uFormat("%d vs %d", windowLeft.rows, windowRight.rows).c_str());
00090         UASSERT_MSG(windowLeft.cols == windowRight.cols, uFormat("%d vs %d", windowLeft.cols, windowRight.cols).c_str());
00091 
00092         float score = 0.0f;
00093         for(int v=0; v<windowLeft.rows; ++v)
00094         {
00095                 for(int u=0; u<windowLeft.cols; ++u)
00096                 {
00097                         if(windowLeft.type() == CV_8UC1)
00098                         {
00099                                 score += fabs(float(windowLeft.at<unsigned char>(v,u))-float(windowRight.at<unsigned char>(v,u)));
00100                         }
00101                         else if(windowLeft.type() == CV_32FC1)
00102                         {
00103                                 score += fabs(windowLeft.at<float>(v,u)-windowRight.at<float>(v,u));
00104                         }
00105                         else if(windowLeft.type() == CV_16SC2)
00106                         {
00107                                 float sL = float(windowLeft.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowLeft.at<cv::Vec2s>(v,u)[1])*0.5f;
00108                                 float sR = float(windowRight.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowRight.at<cv::Vec2s>(v,u)[1])*0.5f;
00109                                 score += fabs(sL - sR);
00110                         }
00111                 }
00112         }
00113         return score;
00114 }
00115 
00116 std::vector<cv::Point2f> calcStereoCorrespondences(
00117                 const cv::Mat & leftImage,
00118                 const cv::Mat & rightImage,
00119                 const std::vector<cv::Point2f> & leftCorners,
00120                 std::vector<unsigned char> & status,
00121                 cv::Size winSize,
00122                 int maxLevel,
00123                 int iterations,
00124                 int minDisparity,
00125                 int maxDisparity,
00126                 bool ssdApproach)
00127 {
00128         UDEBUG("winSize=(%d,%d)", winSize.width, winSize.height);
00129         UDEBUG("maxLevel=%d", maxLevel);
00130         UDEBUG("minDisparity=%d", minDisparity);
00131         UDEBUG("maxDisparity=%d", maxDisparity);
00132         UDEBUG("iterations=%d", iterations);
00133         UDEBUG("ssdApproach=%d", ssdApproach?1:0);
00134 
00135         // window should be odd
00136         if(winSize.width%2 == 0)
00137         {
00138                 winSize.width+=1;
00139         }
00140         if(winSize.height%2 == 0)
00141         {
00142                 winSize.height+=1;
00143         }
00144 
00145         cv::Size halfWin((winSize.width-1)/2, (winSize.height-1)/2);
00146 
00147         UTimer timer;
00148         double pyramidTime = 0.0;
00149         double disparityTime = 0.0;
00150         double subpixelTime = 0.0;
00151 
00152         std::vector<cv::Point2f> rightCorners(leftCorners.size());
00153         std::vector<cv::Mat> leftPyramid, rightPyramid;
00154         maxLevel =  cv::buildOpticalFlowPyramid( leftImage, leftPyramid, winSize, maxLevel, false);
00155         maxLevel =  cv::buildOpticalFlowPyramid( rightImage, rightPyramid, winSize, maxLevel, false);
00156         pyramidTime = timer.ticks();
00157 
00158         status = std::vector<unsigned char>(leftCorners.size(), 0);
00159         int totalIterations = 0;
00160         int noSubPixel = 0;
00161         int added = 0;
00162         for(unsigned int i=0; i<leftCorners.size(); ++i)
00163         {
00164                 int oi=0;
00165                 float bestScore = -1.0f;
00166                 int bestScoreIndex = -1;
00167                 int tmpMinDisparity = minDisparity;
00168                 int tmpMaxDisparity = maxDisparity;
00169 
00170                 int iterations = 0;
00171                 for(int level=maxLevel; level>=0; --level)
00172                 {
00173                         UASSERT(level < (int)leftPyramid.size());
00174 
00175                         cv::Point2i center(int(leftCorners[i].x/float(1<<level)), int(leftCorners[i].y/float(1<<level)));
00176 
00177                         oi=0;
00178                         bestScore = -1.0f;
00179                         bestScoreIndex = -1;
00180                         int localMaxDisparity = -tmpMaxDisparity / (1<<level);
00181                         int localMinDisparity = -tmpMinDisparity / (1<<level);
00182 
00183                         if(center.x-halfWin.width-(level==0?1:0) >=0 && center.x+halfWin.width+(level==0?1:0) < leftPyramid[level].cols &&
00184                            center.y-halfWin.height >=0 && center.y+halfWin.height < leftPyramid[level].rows)
00185                         {
00186                                 cv::Mat windowLeft(leftPyramid[level],
00187                                                 cv::Range(center.y-halfWin.height,center.y+halfWin.height+1),
00188                                                 cv::Range(center.x-halfWin.width,center.x+halfWin.width+1));
00189                                 int minCol = center.x+localMaxDisparity-halfWin.width-1;
00190                                 if(minCol < 0)
00191                                 {
00192                                         localMaxDisparity -= minCol;
00193                                 }
00194 
00195                                 int maxCol = center.x+localMinDisparity+halfWin.width+1;
00196                                 if(maxCol >= leftPyramid[level].cols)
00197                                 {
00198                                         localMinDisparity += maxCol-leftPyramid[level].cols-1;
00199                                 }
00200 
00201                                 if(localMinDisparity < localMaxDisparity)
00202                                 {
00203                                         localMaxDisparity = localMinDisparity;
00204                                 }
00205                                 int length = localMinDisparity-localMaxDisparity+1;
00206                                 std::vector<float> scores = std::vector<float>(length, 0.0f);
00207 
00208                                 for(int d=localMinDisparity; d>localMaxDisparity; --d)
00209                                 {
00210                                         ++iterations;
00211                                         cv::Mat windowRight(rightPyramid[level],
00212                                                                         cv::Range(center.y-halfWin.height,center.y+halfWin.height+1),
00213                                                                         cv::Range(center.x+d-halfWin.width,center.x+d+halfWin.width+1));
00214                                         scores[oi] = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
00215                                         if(scores[oi] > 0 && (bestScore < 0.0f || scores[oi] < bestScore))
00216                                         {
00217                                                 bestScoreIndex = oi;
00218                                                 bestScore = scores[oi];
00219                                         }
00220                                         ++oi;
00221                                 }
00222 
00223                                 if(bestScoreIndex>=0)
00224                                 {
00225                                         if(level>0)
00226                                         {
00227                                                 tmpMaxDisparity = tmpMinDisparity+(bestScoreIndex+1)*(1<<level);
00228                                                 tmpMaxDisparity+=tmpMaxDisparity%level;
00229                                                 if(tmpMaxDisparity > maxDisparity)
00230                                                 {
00231                                                         tmpMaxDisparity = maxDisparity;
00232                                                 }
00233                                                 tmpMinDisparity = tmpMinDisparity+(bestScoreIndex-1)*(1<<level);
00234                                                 tmpMinDisparity -= tmpMinDisparity%level;
00235                                                 if(tmpMinDisparity < minDisparity)
00236                                                 {
00237                                                         tmpMinDisparity = minDisparity;
00238                                                 }
00239                                         }
00240                                 }
00241                         }
00242                 }
00243                 disparityTime+=timer.ticks();
00244                 totalIterations+=iterations;
00245 
00246                 if(bestScoreIndex>=0)
00247                 {
00248                         //subpixel refining
00249                         int d = -(tmpMinDisparity+bestScoreIndex);
00250 
00251                         cv::Mat windowLeft(winSize, CV_32FC1);
00252                         cv::Mat windowRight(winSize, CV_32FC1);
00253                         cv::getRectSubPix(leftPyramid[0],
00254                                         winSize,
00255                                         leftCorners[i],
00256                                         windowLeft,
00257                                         windowLeft.type());
00258                         if(leftCorners[i].x != float(int(leftCorners[i].x)))
00259                         {
00260                                 //recompute bestScore if the pt is not integer
00261                                 cv::getRectSubPix(rightPyramid[0],
00262                                                 winSize,
00263                                                 cv::Point2f(leftCorners[i].x+float(d), leftCorners[i].y),
00264                                                 windowRight,
00265                                                 windowRight.type());
00266                                 bestScore = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
00267                         }
00268 
00269                         float xc = leftCorners[i].x+float(d);
00270                         float vc = bestScore;
00271                         float step = 0.5f;
00272                         std::map<float, float> cache;
00273                         bool reject = false;
00274                         for(int it=0; it<iterations; ++it)
00275                         {
00276                                 float x1 = xc-step;
00277                                 float x2 = xc+step;
00278                                 float v1 = uValue(cache, x1, 0.0f);
00279                                 float v2 = uValue(cache, x2, 0.0f);
00280                                 if(v1 == 0.0f)
00281                                 {
00282                                         cv::getRectSubPix(rightPyramid[0],
00283                                                         winSize,
00284                                                         cv::Point2f(x1, leftCorners[i].y),
00285                                                         windowRight,
00286                                                         windowRight.type());
00287                                         v1 = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
00288                                 }
00289                                 if(v2 == 0.0f)
00290                                 {
00291                                         cv::getRectSubPix(rightPyramid[0],
00292                                                         winSize,
00293                                                         cv::Point2f(x2, leftCorners[i].y),
00294                                                         windowRight,
00295                                                         windowRight.type());
00296                                         v2 = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
00297                                 }
00298 
00299                                 float previousXc = xc;
00300                                 float previousVc = vc;
00301 
00302                                 xc = v1<vc&&v1<v2?x1:v2<vc&&v2<v1?x2:xc;
00303                                 vc = v1<vc&&v1<v2?v1:v2<vc&&v2<v1?v2:vc;
00304 
00305                                 if(previousXc == xc)
00306                                 {
00307                                         step /= 2.0f;
00308                                 }
00309                                 else
00310                                 {
00311                                         cache.insert(std::make_pair(previousXc, previousVc));
00312                                 }
00313 
00314                                 if(xc < leftCorners[i].x+float(d)-1.0f || xc > leftCorners[i].x+float(d)+1.0f)
00315                                 {
00316                                         reject = true;
00317                                         break;
00318                                 }
00319                         }
00320 
00321                         rightCorners[i] = cv::Point2f(xc, leftCorners[i].y);
00322                         status[i] = reject?0:1;
00323                         if(!reject)
00324                         {
00325                                 if(leftCorners[i].x+float(d) != xc)
00326                                 {
00327                                         ++noSubPixel;
00328                                 }
00329                                 ++added;
00330                         }
00331                 }
00332                 subpixelTime+=timer.ticks();
00333         }
00334         UDEBUG("SubPixel=%d/%d added (total=%d)", noSubPixel, added, (int)status.size());
00335         UDEBUG("totalIterations=%d", totalIterations);
00336         UDEBUG("Time pyramid = %f s", pyramidTime);
00337         UDEBUG("Time disparity = %f s", disparityTime);
00338         UDEBUG("Time sub-pixel = %f s", subpixelTime);
00339 
00340         return rightCorners;
00341 }
00342 
00343 typedef float acctype;
00344 typedef float itemtype;
00345 #define  CV_DESCALE(x,n)     (((x) + (1 << ((n)-1))) >> (n))
00346 
00347 //
00348 // Adapted from OpenCV cv::calcOpticalFlowPyrLK() to force
00349 // only optical flow on x-axis (assuming that prevImg is the left
00350 // image and nextImg is the right image):
00351 // https://github.com/Itseez/opencv/blob/ddf82d0b154873510802ef75c53e628cd7b2cb13/modules/video/src/lkpyramid.cpp#L1088
00352 //
00353 // The difference is on this line:
00354 // https://github.com/Itseez/opencv/blob/ddf82d0b154873510802ef75c53e628cd7b2cb13/modules/video/src/lkpyramid.cpp#L683-L684
00355 // - cv::Point2f delta( (float)((A12*b2 - A22*b1) * D), (float)((A12*b1 - A11*b2) * D));
00356 // + cv::Point2f delta( (float)((A12*b2 - A22*b1) * D), 0); //<--- note the 0 for y
00357 //
00358 void calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputArray _nextImg,
00359                            cv::InputArray _prevPts, cv::InputOutputArray _nextPts,
00360                            cv::OutputArray _status, cv::OutputArray _err,
00361                            cv::Size winSize, int maxLevel,
00362                            cv::TermCriteria criteria,
00363                            int flags, double minEigThreshold )
00364 {
00365     cv::Mat prevPtsMat = _prevPts.getMat();
00366     const int derivDepth = cv::DataType<short>::depth;
00367 
00368     CV_Assert( maxLevel >= 0 && winSize.width > 2 && winSize.height > 2 );
00369 
00370     int level=0, i, npoints;
00371     CV_Assert( (npoints = prevPtsMat.checkVector(2, CV_32F, true)) >= 0 );
00372 
00373     if( npoints == 0 )
00374     {
00375         _nextPts.release();
00376         _status.release();
00377         _err.release();
00378         return;
00379     }
00380 
00381     if( !(flags & cv::OPTFLOW_USE_INITIAL_FLOW) )
00382         _nextPts.create(prevPtsMat.size(), prevPtsMat.type(), -1, true);
00383 
00384     cv::Mat nextPtsMat = _nextPts.getMat();
00385     CV_Assert( nextPtsMat.checkVector(2, CV_32F, true) == npoints );
00386 
00387     const cv::Point2f* prevPts = prevPtsMat.ptr<cv::Point2f>();
00388     cv::Point2f* nextPts = nextPtsMat.ptr<cv::Point2f>();
00389 
00390     _status.create((int)npoints, 1, CV_8U, -1, true);
00391     cv::Mat statusMat = _status.getMat(), errMat;
00392     CV_Assert( statusMat.isContinuous() );
00393     uchar* status = statusMat.ptr();
00394     float* err = 0;
00395 
00396     for( i = 0; i < npoints; i++ )
00397         status[i] = true;
00398 
00399     if( _err.needed() )
00400     {
00401         _err.create((int)npoints, 1, CV_32F, -1, true);
00402         errMat = _err.getMat();
00403         CV_Assert( errMat.isContinuous() );
00404         err = errMat.ptr<float>();
00405     }
00406 
00407     std::vector<cv::Mat> prevPyr, nextPyr;
00408     int levels1 = -1;
00409     int lvlStep1 = 1;
00410     int levels2 = -1;
00411     int lvlStep2 = 1;
00412 
00413 
00414     if(_prevImg.kind() != cv::_InputArray::STD_VECTOR_MAT)
00415         {
00416                 //create pyramid
00417                 maxLevel =  cv::buildOpticalFlowPyramid(_prevImg, prevPyr, winSize, maxLevel, true);
00418         }
00419         else if(_prevImg.kind() == cv::_InputArray::STD_VECTOR_MAT)
00420         {
00421                 _prevImg.getMatVector(prevPyr);
00422         }
00423 
00424         levels1 = int(prevPyr.size()) - 1;
00425         CV_Assert(levels1 >= 0);
00426 
00427         if (levels1 % 2 == 1 && prevPyr[0].channels() * 2 == prevPyr[1].channels() && prevPyr[1].depth() == derivDepth)
00428         {
00429                 lvlStep1 = 2;
00430                 levels1 /= 2;
00431         }
00432 
00433         // ensure that pyramid has required padding
00434         if(levels1 > 0)
00435         {
00436                 cv::Size fullSize;
00437                 cv::Point ofs;
00438                 prevPyr[lvlStep1].locateROI(fullSize, ofs);
00439                 CV_Assert(ofs.x >= winSize.width && ofs.y >= winSize.height
00440                         && ofs.x + prevPyr[lvlStep1].cols + winSize.width <= fullSize.width
00441                         && ofs.y + prevPyr[lvlStep1].rows + winSize.height <= fullSize.height);
00442         }
00443 
00444         if(levels1 < maxLevel)
00445                 maxLevel = levels1;
00446 
00447         if(_nextImg.kind() != cv::_InputArray::STD_VECTOR_MAT)
00448         {
00449                 //create pyramid
00450                 maxLevel =  cv::buildOpticalFlowPyramid(_nextImg, nextPyr, winSize, maxLevel, false);
00451         }
00452         else if(_nextImg.kind() == cv::_InputArray::STD_VECTOR_MAT)
00453     {
00454         _nextImg.getMatVector(nextPyr);
00455     }
00456 
00457         levels2 = int(nextPyr.size()) - 1;
00458         CV_Assert(levels2 >= 0);
00459 
00460         if (levels2 % 2 == 1 && nextPyr[0].channels() * 2 == nextPyr[1].channels() && nextPyr[1].depth() == derivDepth)
00461         {
00462                 lvlStep2 = 2;
00463                 levels2 /= 2;
00464         }
00465 
00466         // ensure that pyramid has required padding
00467         if(levels2 > 0)
00468         {
00469                 cv::Size fullSize;
00470                 cv::Point ofs;
00471                 nextPyr[lvlStep2].locateROI(fullSize, ofs);
00472                 CV_Assert(ofs.x >= winSize.width && ofs.y >= winSize.height
00473                         && ofs.x + nextPyr[lvlStep2].cols + winSize.width <= fullSize.width
00474                         && ofs.y + nextPyr[lvlStep2].rows + winSize.height <= fullSize.height);
00475         }
00476 
00477         if(levels2 < maxLevel)
00478                 maxLevel = levels2;
00479 
00480     if( (criteria.type & cv::TermCriteria::COUNT) == 0 )
00481         criteria.maxCount = 30;
00482     else
00483         criteria.maxCount = std::min(std::max(criteria.maxCount, 0), 100);
00484     if( (criteria.type & cv::TermCriteria::EPS) == 0 )
00485         criteria.epsilon = 0.01;
00486     else
00487         criteria.epsilon = std::min(std::max(criteria.epsilon, 0.), 10.);
00488     criteria.epsilon *= criteria.epsilon;
00489 
00490     // for all pyramids
00491     for( level = maxLevel; level >= 0; level-- )
00492     {
00493         cv::Mat derivI = prevPyr[level * lvlStep1 + 1];
00494 
00495         CV_Assert(prevPyr[level * lvlStep1].size() == nextPyr[level * lvlStep2].size());
00496         CV_Assert(prevPyr[level * lvlStep1].type() == nextPyr[level * lvlStep2].type());
00497 
00498         const cv::Mat & prevImg = prevPyr[level * lvlStep1];
00499         const cv::Mat & prevDeriv = derivI;
00500         const cv::Mat & nextImg = nextPyr[level * lvlStep2];
00501 
00502         // for all corners
00503         {
00504                 cv::Point2f halfWin((winSize.width-1)*0.5f, (winSize.height-1)*0.5f);
00505                         const cv::Mat& I = prevImg;
00506                         const cv::Mat& J = nextImg;
00507                         const cv::Mat& derivI = prevDeriv;
00508 
00509                         int j, cn = I.channels(), cn2 = cn*2;
00510                         cv::AutoBuffer<short> _buf(winSize.area()*(cn + cn2));
00511                         int derivDepth = cv::DataType<short>::depth;
00512 
00513                         cv::Mat IWinBuf(winSize, CV_MAKETYPE(derivDepth, cn), (short*)_buf);
00514                         cv::Mat derivIWinBuf(winSize, CV_MAKETYPE(derivDepth, cn2), (short*)_buf + winSize.area()*cn);
00515 
00516                         for( int ptidx = 0; ptidx < npoints; ptidx++ )
00517                         {
00518                                 cv::Point2f prevPt = prevPts[ptidx]*(float)(1./(1 << level));
00519                                 cv::Point2f nextPt;
00520                                 if( level == maxLevel )
00521                                 {
00522                                         if( flags & cv::OPTFLOW_USE_INITIAL_FLOW )
00523                                                 nextPt = nextPts[ptidx]*(float)(1./(1 << level));
00524                                         else
00525                                                 nextPt = prevPt;
00526                                 }
00527                                 else
00528                                         nextPt = nextPts[ptidx]*2.f;
00529                                 nextPts[ptidx] = nextPt;
00530 
00531                                 cv::Point2i iprevPt, inextPt;
00532                                 prevPt -= halfWin;
00533                                 iprevPt.x = cvFloor(prevPt.x);
00534                                 iprevPt.y = cvFloor(prevPt.y);
00535 
00536                                 if( iprevPt.x < -winSize.width || iprevPt.x >= derivI.cols ||
00537                                         iprevPt.y < -winSize.height || iprevPt.y >= derivI.rows )
00538                                 {
00539                                         if( level == 0 )
00540                                         {
00541                                                 if( status )
00542                                                         status[ptidx] = false;
00543                                                 if( err )
00544                                                         err[ptidx] = 0;
00545                                         }
00546                                         continue;
00547                                 }
00548 
00549                                 float a = prevPt.x - iprevPt.x;
00550                                 float b = prevPt.y - iprevPt.y;
00551                                 const int W_BITS = 14, W_BITS1 = 14;
00552                                 const float FLT_SCALE = 1.f/(1 << 20);
00553                                 int iw00 = cvRound((1.f - a)*(1.f - b)*(1 << W_BITS));
00554                                 int iw01 = cvRound(a*(1.f - b)*(1 << W_BITS));
00555                                 int iw10 = cvRound((1.f - a)*b*(1 << W_BITS));
00556                                 int iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
00557 
00558                                 int dstep = (int)(derivI.step/derivI.elemSize1());
00559                                 int stepI = (int)(I.step/I.elemSize1());
00560                                 int stepJ = (int)(J.step/J.elemSize1());
00561                                 acctype iA11 = 0, iA12 = 0, iA22 = 0;
00562                                 float A11, A12, A22;
00563 
00564                                 // extract the patch from the first image, compute covariation cv::Matrix of derivatives
00565                                 int x, y;
00566                                 for( y = 0; y < winSize.height; y++ )
00567                                 {
00568                                         const uchar* src = I.ptr() + (y + iprevPt.y)*stepI + iprevPt.x*cn;
00569                                         const short* dsrc = derivI.ptr<short>() + (y + iprevPt.y)*dstep + iprevPt.x*cn2;
00570 
00571                                         short* Iptr = IWinBuf.ptr<short>(y);
00572                                         short* dIptr = derivIWinBuf.ptr<short>(y);
00573 
00574                                         x = 0;
00575 
00576                                         for( ; x < winSize.width*cn; x++, dsrc += 2, dIptr += 2 )
00577                                         {
00578                                                 int ival = CV_DESCALE(src[x]*iw00 + src[x+cn]*iw01 +
00579                                                                                           src[x+stepI]*iw10 + src[x+stepI+cn]*iw11, W_BITS1-5);
00580                                                 int ixval = CV_DESCALE(dsrc[0]*iw00 + dsrc[cn2]*iw01 +
00581                                                                                            dsrc[dstep]*iw10 + dsrc[dstep+cn2]*iw11, W_BITS1);
00582                                                 int iyval = CV_DESCALE(dsrc[1]*iw00 + dsrc[cn2+1]*iw01 + dsrc[dstep+1]*iw10 +
00583                                                                                            dsrc[dstep+cn2+1]*iw11, W_BITS1);
00584 
00585                                                 Iptr[x] = (short)ival;
00586                                                 dIptr[0] = (short)ixval;
00587                                                 dIptr[1] = (short)iyval;
00588 
00589                                                 iA11 += (itemtype)(ixval*ixval);
00590                                                 iA12 += (itemtype)(ixval*iyval);
00591                                                 iA22 += (itemtype)(iyval*iyval);
00592                                         }
00593                                 }
00594 
00595                                 A11 = iA11*FLT_SCALE;
00596                                 A12 = iA12*FLT_SCALE;
00597                                 A22 = iA22*FLT_SCALE;
00598 
00599                                 float D = A11*A22 - A12*A12;
00600                                 float minEig = (A22 + A11 - std::sqrt((A11-A22)*(A11-A22) +
00601                                                                 4.f*A12*A12))/(2*winSize.width*winSize.height);
00602 
00603                                 if( err && (flags & cv::OPTFLOW_LK_GET_MIN_EIGENVALS) != 0 )
00604                                         err[ptidx] = (float)minEig;
00605 
00606                                 if( minEig < minEigThreshold || D < FLT_EPSILON )
00607                                 {
00608                                         if( level == 0 && status )
00609                                                 status[ptidx] = false;
00610                                         continue;
00611                                 }
00612 
00613                                 D = 1.f/D;
00614 
00615                                 nextPt -= halfWin;
00616                                 cv::Point2f prevDelta;
00617 
00618                                 for( j = 0; j < criteria.maxCount; j++ )
00619                                 {
00620                                         inextPt.x = cvFloor(nextPt.x);
00621                                         inextPt.y = cvFloor(nextPt.y);
00622 
00623                                         if( inextPt.x < -winSize.width || inextPt.x >= J.cols ||
00624                                            inextPt.y < -winSize.height || inextPt.y >= J.rows )
00625                                         {
00626                                                 if( level == 0 && status )
00627                                                         status[ptidx] = false;
00628                                                 break;
00629                                         }
00630 
00631                                         a = nextPt.x - inextPt.x;
00632                                         b = nextPt.y - inextPt.y;
00633                                         iw00 = cvRound((1.f - a)*(1.f - b)*(1 << W_BITS));
00634                                         iw01 = cvRound(a*(1.f - b)*(1 << W_BITS));
00635                                         iw10 = cvRound((1.f - a)*b*(1 << W_BITS));
00636                                         iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
00637                                         acctype ib1 = 0, ib2 = 0;
00638                                         float b1, b2;
00639 
00640                                         for( y = 0; y < winSize.height; y++ )
00641                                         {
00642                                                 const uchar* Jptr = J.ptr() + (y + inextPt.y)*stepJ + inextPt.x*cn;
00643                                                 const short* Iptr = IWinBuf.ptr<short>(y);
00644                                                 const short* dIptr = derivIWinBuf.ptr<short>(y);
00645 
00646                                                 x = 0;
00647 
00648                                                 for( ; x < winSize.width*cn; x++, dIptr += 2 )
00649                                                 {
00650                                                         int diff = CV_DESCALE(Jptr[x]*iw00 + Jptr[x+cn]*iw01 +
00651                                                                                                   Jptr[x+stepJ]*iw10 + Jptr[x+stepJ+cn]*iw11,
00652                                                                                                   W_BITS1-5) - Iptr[x];
00653                                                         ib1 += (itemtype)(diff*dIptr[0]);
00654                                                         ib2 += (itemtype)(diff*dIptr[1]);
00655                                                 }
00656                                         }
00657 
00658                                         b1 = ib1*FLT_SCALE;
00659                                         b2 = ib2*FLT_SCALE;
00660 
00661                                         cv::Point2f delta( (float)((A12*b2 - A22*b1) * D),
00662                                                                   0);//(float)((A12*b1 - A11*b2) * D)); // MODIFICATION
00663                                         //delta = -delta;
00664 
00665                                         nextPt += delta;
00666                                         nextPts[ptidx] = nextPt + halfWin;
00667 
00668                                         if( delta.ddot(delta) <= criteria.epsilon )
00669                                                 break;
00670 
00671                                         if( j > 0 && std::abs(delta.x + prevDelta.x) < 0.01 &&
00672                                            std::abs(delta.y + prevDelta.y) < 0.01 )
00673                                         {
00674                                                 nextPts[ptidx] -= delta*0.5f;
00675                                                 break;
00676                                         }
00677                                         prevDelta = delta;
00678                                 }
00679 
00680                                 if( status[ptidx] && err && level == 0 && (flags & cv::OPTFLOW_LK_GET_MIN_EIGENVALS) == 0 )
00681                                 {
00682                                         cv::Point2f nextPoint = nextPts[ptidx] - halfWin;
00683                                         cv::Point inextPoint;
00684 
00685                                         inextPoint.x = cvFloor(nextPoint.x);
00686                                         inextPoint.y = cvFloor(nextPoint.y);
00687 
00688                                         if( inextPoint.x < -winSize.width || inextPoint.x >= J.cols ||
00689                                                 inextPoint.y < -winSize.height || inextPoint.y >= J.rows )
00690                                         {
00691                                                 if( status )
00692                                                         status[ptidx] = false;
00693                                                 continue;
00694                                         }
00695 
00696                                         float aa = nextPoint.x - inextPoint.x;
00697                                         float bb = nextPoint.y - inextPoint.y;
00698                                         iw00 = cvRound((1.f - aa)*(1.f - bb)*(1 << W_BITS));
00699                                         iw01 = cvRound(aa*(1.f - bb)*(1 << W_BITS));
00700                                         iw10 = cvRound((1.f - aa)*bb*(1 << W_BITS));
00701                                         iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
00702                                         float errval = 0.f;
00703 
00704                                         for( y = 0; y < winSize.height; y++ )
00705                                         {
00706                                                 const uchar* Jptr = J.ptr() + (y + inextPoint.y)*stepJ + inextPoint.x*cn;
00707                                                 const short* Iptr = IWinBuf.ptr<short>(y);
00708 
00709                                                 for( x = 0; x < winSize.width*cn; x++ )
00710                                                 {
00711                                                         int diff = CV_DESCALE(Jptr[x]*iw00 + Jptr[x+cn]*iw01 +
00712                                                                                                   Jptr[x+stepJ]*iw10 + Jptr[x+stepJ+cn]*iw11,
00713                                                                                                   W_BITS1-5) - Iptr[x];
00714                                                         errval += std::abs((float)diff);
00715                                                 }
00716                                         }
00717                                         err[ptidx] = errval * 1.f/(32*winSize.width*cn*winSize.height);
00718                                 }
00719                         }
00720         }
00721 
00722     }
00723 }
00724 
00725 cv::Mat disparityFromStereoImages(
00726                 const cv::Mat & leftImage,
00727                 const cv::Mat & rightImage,
00728                 const ParametersMap & parameters)
00729 {
00730         UASSERT(!leftImage.empty() && !rightImage.empty());
00731         UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
00732         UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1);
00733 
00734         cv::Mat leftMono;
00735         if(leftImage.channels() == 3)
00736         {
00737                 cv::cvtColor(leftImage, leftMono, CV_BGR2GRAY);
00738         }
00739         else
00740         {
00741                 leftMono = leftImage;
00742         }
00743         cv::Mat disparity;
00744 
00745         StereoBM stereo(parameters);
00746         return stereo.computeDisparity(leftMono, rightImage);
00747 }
00748 
00749 cv::Mat depthFromDisparity(const cv::Mat & disparity,
00750                 float fx, float baseline,
00751                 int type)
00752 {
00753         UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
00754         UASSERT(type == CV_32FC1 || type == CV_16UC1);
00755         cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type);
00756         int countOverMax = 0;
00757         for (int i = 0; i < disparity.rows; i++)
00758         {
00759                 for (int j = 0; j < disparity.cols; j++)
00760                 {
00761                         float disparity_value = disparity.type() == CV_16SC1?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
00762                         if (disparity_value > 0.0f)
00763                         {
00764                                 // baseline * focal / disparity
00765                                 float d = baseline * fx / disparity_value;
00766                                 if(d>0)
00767                                 {
00768                                         if(depth.type() == CV_32FC1)
00769                                         {
00770                                                 depth.at<float>(i,j) = d;
00771                                         }
00772                                         else
00773                                         {
00774                                                 if(d*1000.0f <= (float)USHRT_MAX)
00775                                                 {
00776                                                         depth.at<unsigned short>(i,j) = (unsigned short)(d*1000.0f);
00777                                                 }
00778                                                 else
00779                                                 {
00780                                                         ++countOverMax;
00781                                                 }
00782                                         }
00783                                 }
00784                         }
00785                 }
00786         }
00787         if(countOverMax)
00788         {
00789                 UWARN("Depth conversion error, %d depth values ignored because they are over the maximum depth allowed (65535 mm).", countOverMax);
00790         }
00791         return depth;
00792 }
00793 
00794 cv::Mat depthFromStereoImages(
00795                 const cv::Mat & leftImage,
00796                 const cv::Mat & rightImage,
00797                 const std::vector<cv::Point2f> & leftCorners,
00798                 float fx,
00799                 float baseline,
00800                 int flowWinSize,
00801                 int flowMaxLevel,
00802                 int flowIterations,
00803                 double flowEps)
00804 {
00805         UASSERT(!leftImage.empty() && !rightImage.empty() &&
00806                         leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
00807                         leftImage.cols == rightImage.cols &&
00808                         leftImage.rows == rightImage.rows);
00809         UASSERT(fx > 0.0f && baseline > 0.0f);
00810 
00811         // Find features in the new left image
00812         std::vector<unsigned char> status;
00813         std::vector<float> err;
00814         std::vector<cv::Point2f> rightCorners;
00815         UDEBUG("cv::calcOpticalFlowPyrLK() begin");
00816         cv::calcOpticalFlowPyrLK(
00817                         leftImage,
00818                         rightImage,
00819                         leftCorners,
00820                         rightCorners,
00821                         status,
00822                         err,
00823                         cv::Size(flowWinSize, flowWinSize), flowMaxLevel,
00824                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations, flowEps),
00825                         cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
00826         UDEBUG("cv::calcOpticalFlowPyrLK() end");
00827 
00828         return depthFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, fx, baseline);
00829 }
00830 
00831 cv::Mat disparityFromStereoCorrespondences(
00832                 const cv::Size & disparitySize,
00833                 const std::vector<cv::Point2f> & leftCorners,
00834                 const std::vector<cv::Point2f> & rightCorners,
00835                 const std::vector<unsigned char> & mask)
00836 {
00837         UASSERT(leftCorners.size() == rightCorners.size());
00838         UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
00839         cv::Mat disparity = cv::Mat::zeros(disparitySize, CV_32FC1);
00840         for(unsigned int i=0; i<leftCorners.size(); ++i)
00841         {
00842                 if(mask.empty() || mask[i])
00843                 {
00844                         cv::Point2i dispPt(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f));
00845                         UASSERT(dispPt.x >= 0 && dispPt.x < disparitySize.width);
00846                         UASSERT(dispPt.y >= 0 && dispPt.y < disparitySize.height);
00847                         disparity.at<float>(dispPt.y, dispPt.x) = leftCorners[i].x - rightCorners[i].x;
00848                 }
00849         }
00850         return disparity;
00851 }
00852 
00853 cv::Mat depthFromStereoCorrespondences(
00854                 const cv::Mat & leftImage,
00855                 const std::vector<cv::Point2f> & leftCorners,
00856                 const std::vector<cv::Point2f> & rightCorners,
00857                 const std::vector<unsigned char> & mask,
00858                 float fx, float baseline)
00859 {
00860         UASSERT(!leftImage.empty() && leftCorners.size() == rightCorners.size());
00861         UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
00862         cv::Mat depth = cv::Mat::zeros(leftImage.rows, leftImage.cols, CV_32FC1);
00863         for(unsigned int i=0; i<leftCorners.size(); ++i)
00864         {
00865                 if(mask.size() == 0 || mask[i])
00866                 {
00867                         float disparity = leftCorners[i].x - rightCorners[i].x;
00868                         if(disparity > 0.0f)
00869                         {
00870                                 float d = baseline * fx / disparity;
00871                                 depth.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
00872                         }
00873                 }
00874         }
00875         return depth;
00876 }
00877 
00878 cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
00879 {
00880         UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
00881         cv::Mat depth16U;
00882         if(!depth32F.empty())
00883         {
00884                 depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
00885                 int countOverMax = 0;
00886                 for(int i=0; i<depth32F.rows; ++i)
00887                 {
00888                         for(int j=0; j<depth32F.cols; ++j)
00889                         {
00890                                 float depth = (depth32F.at<float>(i,j)*1000.0f);
00891                                 unsigned short depthMM = 0;
00892                                 if(depth > 0 && depth <= (float)USHRT_MAX)
00893                                 {
00894                                         depthMM = (unsigned short)depth;
00895                                 }
00896                                 else if(depth > (float)USHRT_MAX)
00897                                 {
00898                                         ++countOverMax;
00899                                 }
00900                                 depth16U.at<unsigned short>(i, j) = depthMM;
00901                         }
00902                 }
00903                 if(countOverMax)
00904                 {
00905                         UWARN("Depth conversion error, %d depth values ignored because "
00906                                   "they are over the maximum depth allowed (65535 mm). Is the depth "
00907                                   "image really in meters? 32 bits images should be in meters, "
00908                                   "and 16 bits should be in mm.", countOverMax);
00909                 }
00910         }
00911         return depth16U;
00912 }
00913 
00914 cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
00915 {
00916         UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
00917         cv::Mat depth32F;
00918         if(!depth16U.empty())
00919         {
00920                 depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
00921                 for(int i=0; i<depth16U.rows; ++i)
00922                 {
00923                         for(int j=0; j<depth16U.cols; ++j)
00924                         {
00925                                 float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
00926                                 depth32F.at<float>(i, j) = depth;
00927                         }
00928                 }
00929         }
00930         return depth32F;
00931 }
00932 
00933 float getDepth(
00934                 const cv::Mat & depthImage,
00935                 float x, float y,
00936                 bool smoothing,
00937                 float maxZError,
00938                 bool estWithNeighborsIfNull)
00939 {
00940         UASSERT(!depthImage.empty());
00941         UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
00942 
00943         int u = int(x+0.5f);
00944         int v = int(y+0.5f);
00945         if(u == depthImage.cols && x<float(depthImage.cols))
00946         {
00947                 u = depthImage.cols - 1;
00948         }
00949         if(v == depthImage.rows && y<float(depthImage.rows))
00950         {
00951                 v = depthImage.rows - 1;
00952         }
00953 
00954         if(!(u >=0 && u<depthImage.cols && v >=0 && v<depthImage.rows))
00955         {
00956                 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)",
00957                                 x,u,y,v,depthImage.cols, depthImage.rows);
00958                 return 0;
00959         }
00960 
00961         bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
00962 
00963         // Inspired from RGBDFrame::getGaussianMixtureDistribution() method from
00964         // https://github.com/ccny-ros-pkg/rgbdtools/blob/master/src/rgbd_frame.cpp
00965         // Window weights:
00966         //  | 1 | 2 | 1 |
00967         //  | 2 | 4 | 2 |
00968         //  | 1 | 2 | 1 |
00969         int u_start = std::max(u-1, 0);
00970         int v_start = std::max(v-1, 0);
00971         int u_end = std::min(u+1, depthImage.cols-1);
00972         int v_end = std::min(v+1, depthImage.rows-1);
00973 
00974         float depth = 0.0f;
00975         if(isInMM)
00976         {
00977                 if(depthImage.at<unsigned short>(v,u) > 0 &&
00978                    depthImage.at<unsigned short>(v,u) < std::numeric_limits<unsigned short>::max())
00979                 {
00980                         depth = float(depthImage.at<unsigned short>(v,u))*0.001f;
00981                 }
00982         }
00983         else
00984         {
00985                 depth = depthImage.at<float>(v,u);
00986         }
00987 
00988         if((depth==0.0f || !uIsFinite(depth)) && estWithNeighborsIfNull)
00989         {
00990                 // all cells no2 must be under the zError to be accepted
00991                 float tmp = 0.0f;
00992                 int count = 0;
00993                 for(int uu = u_start; uu <= u_end; ++uu)
00994                 {
00995                         for(int vv = v_start; vv <= v_end; ++vv)
00996                         {
00997                                 if((uu == u && vv!=v) || (uu != u && vv==v))
00998                                 {
00999                                         float d = 0.0f;
01000                                         if(isInMM)
01001                                         {
01002                                                 if(depthImage.at<unsigned short>(vv,uu) > 0 &&
01003                                                    depthImage.at<unsigned short>(vv,uu) < std::numeric_limits<unsigned short>::max())
01004                                                 {
01005                                                         d = float(depthImage.at<unsigned short>(vv,uu))*0.001f;
01006                                                 }
01007                                         }
01008                                         else
01009                                         {
01010                                                 d = depthImage.at<float>(vv,uu);
01011                                         }
01012                                         if(d!=0.0f && uIsFinite(d))
01013                                         {
01014                                                 if(tmp == 0.0f)
01015                                                 {
01016                                                         tmp = d;
01017                                                         ++count;
01018                                                 }
01019                                                 else if(fabs(d - tmp) < maxZError)
01020                                                 {
01021                                                         tmp+=d;
01022                                                         ++count;
01023                                                 }
01024                                         }
01025                                 }
01026                         }
01027                 }
01028                 if(count > 1)
01029                 {
01030                         depth = tmp/float(count);
01031                 }
01032         }
01033 
01034         if(depth!=0.0f && uIsFinite(depth))
01035         {
01036                 if(smoothing)
01037                 {
01038                         float sumWeights = 0.0f;
01039                         float sumDepths = 0.0f;
01040                         for(int uu = u_start; uu <= u_end; ++uu)
01041                         {
01042                                 for(int vv = v_start; vv <= v_end; ++vv)
01043                                 {
01044                                         if(!(uu == u && vv == v))
01045                                         {
01046                                                 float d = 0.0f;
01047                                                 if(isInMM)
01048                                                 {
01049                                                         if(depthImage.at<unsigned short>(vv,uu) > 0 &&
01050                                                            depthImage.at<unsigned short>(vv,uu) < std::numeric_limits<unsigned short>::max())
01051                                                         {
01052                                                                 d = float(depthImage.at<unsigned short>(vv,uu))*0.001f;
01053                                                         }
01054                                                 }
01055                                                 else
01056                                                 {
01057                                                         d = depthImage.at<float>(vv,uu);
01058                                                 }
01059 
01060                                                 // ignore if not valid or depth difference is too high
01061                                                 if(d != 0.0f && uIsFinite(d) && fabs(d - depth) < maxZError)
01062                                                 {
01063                                                         if(uu == u || vv == v)
01064                                                         {
01065                                                                 sumWeights+=2.0f;
01066                                                                 d*=2.0f;
01067                                                         }
01068                                                         else
01069                                                         {
01070                                                                 sumWeights+=1.0f;
01071                                                         }
01072                                                         sumDepths += d;
01073                                                 }
01074                                         }
01075                                 }
01076                         }
01077                         // set window weight to center point
01078                         depth *= 4.0f;
01079                         sumWeights += 4.0f;
01080 
01081                         // mean
01082                         depth = (depth+sumDepths)/sumWeights;
01083                 }
01084         }
01085         else
01086         {
01087                 depth = 0;
01088         }
01089         return depth;
01090 }
01091 
01092 cv::Mat decimate(const cv::Mat & image, int decimation)
01093 {
01094         UASSERT(decimation >= 1);
01095         cv::Mat out;
01096         if(!image.empty())
01097         {
01098                 if(decimation > 1)
01099                 {
01100                         if((image.type() == CV_32FC1 || image.type()==CV_16UC1))
01101                         {
01102                                 UASSERT_MSG(image.rows % decimation == 0 && image.cols % decimation == 0, "Decimation of depth images should be exact!");
01103 
01104                                 out = cv::Mat(image.rows/decimation, image.cols/decimation, image.type());
01105                                 if(image.type() == CV_32FC1)
01106                                 {
01107                                         for(int j=0; j<out.rows; ++j)
01108                                         {
01109                                                 for(int i=0; i<out.cols; ++i)
01110                                                 {
01111                                                         out.at<float>(j, i) = image.at<float>(j*decimation, i*decimation);
01112                                                 }
01113                                         }
01114                                 }
01115                                 else // CV_16UC1
01116                                 {
01117                                         for(int j=0; j<out.rows; ++j)
01118                                         {
01119                                                 for(int i=0; i<out.cols; ++i)
01120                                                 {
01121                                                         out.at<unsigned short>(j, i) = image.at<unsigned short>(j*decimation, i*decimation);
01122                                                 }
01123                                         }
01124                                 }
01125                         }
01126                         else
01127                         {
01128                                 cv::resize(image, out, cv::Size(), 1.0f/float(decimation), 1.0f/float(decimation), cv::INTER_AREA);
01129                         }
01130                 }
01131                 else
01132                 {
01133                         out = image;
01134                 }
01135         }
01136         return out;
01137 }
01138 
01139 cv::Mat interpolate(const cv::Mat & image, int factor, float depthErrorRatio)
01140 {
01141         UASSERT(factor >= 1);
01142         cv::Mat out;
01143         if(!image.empty())
01144         {
01145                 if(factor > 1)
01146                 {
01147                         if((image.type() == CV_32FC1 || image.type()==CV_16UC1))
01148                         {
01149                                 UASSERT(depthErrorRatio>0.0f);
01150                                 out = cv::Mat::zeros(image.rows*factor, image.cols*factor, image.type());
01151                                 for(int j=0; j<out.rows; j+=factor)
01152                                 {
01153                                         for(int i=0; i<out.cols; i+=factor)
01154                                         {
01155                                                 if(i>0 && j>0)
01156                                                 {
01157                                                         float dTopLeft;
01158                                                         float dTopRight;
01159                                                         float dBottomLeft;
01160                                                         float dBottomRight;
01161                                                         if(image.type() == CV_32FC1)
01162                                                         {
01163                                                                 dTopLeft = image.at<float>(j/factor-1, i/factor-1);
01164                                                                 dTopRight = image.at<float>(j/factor-1, i/factor);
01165                                                                 dBottomLeft = image.at<float>(j/factor, i/factor-1);
01166                                                                 dBottomRight = image.at<float>(j/factor, i/factor);
01167                                                         }
01168                                                         else
01169                                                         {
01170                                                                 dTopLeft = image.at<unsigned short>(j/factor-1, i/factor-1);
01171                                                                 dTopRight = image.at<unsigned short>(j/factor-1, i/factor);
01172                                                                 dBottomLeft = image.at<unsigned short>(j/factor, i/factor-1);
01173                                                                 dBottomRight = image.at<unsigned short>(j/factor, i/factor);
01174                                                         }
01175 
01176                                                         if(dTopLeft>0 && dTopRight>0 && dBottomLeft>0 && dBottomRight > 0)
01177                                                         {
01178                                                                 float depthError = depthErrorRatio*(dTopLeft+dTopRight+dBottomLeft+dBottomRight)/4.0f;
01179                                                                 if(fabs(dTopLeft-dTopRight) <= depthError &&
01180                                                                    fabs(dTopLeft-dBottomLeft) <= depthError &&
01181                                                                    fabs(dTopLeft-dBottomRight) <= depthError)
01182                                                                 {
01183                                                                         // bilinear interpolation
01184                                                                         // do first and last rows then columns
01185                                                                         float slopeTop = (dTopRight-dTopLeft)/float(factor);
01186                                                                         float slopeBottom = (dBottomRight-dBottomLeft)/float(factor);
01187                                                                         if(image.type() == CV_32FC1)
01188                                                                         {
01189                                                                                 for(int z=i-factor; z<=i; ++z)
01190                                                                                 {
01191                                                                                         out.at<float>(j-factor, z) = dTopLeft+(slopeTop*float(z-(i-factor)));
01192                                                                                         out.at<float>(j, z) = dBottomLeft+(slopeBottom*float(z-(i-factor)));
01193                                                                                 }
01194                                                                         }
01195                                                                         else
01196                                                                         {
01197                                                                                 for(int z=i-factor; z<=i; ++z)
01198                                                                                 {
01199                                                                                         out.at<unsigned short>(j-factor, z) = (unsigned short)(dTopLeft+(slopeTop*float(z-(i-factor))));
01200                                                                                         out.at<unsigned short>(j, z) = (unsigned short)(dBottomLeft+(slopeBottom*float(z-(i-factor))));
01201                                                                                 }
01202                                                                         }
01203 
01204                                                                         // fill the columns
01205                                                                         if(image.type() == CV_32FC1)
01206                                                                         {
01207                                                                                 for(int z=i-factor; z<=i; ++z)
01208                                                                                 {
01209                                                                                         float top = out.at<float>(j-factor, z);
01210                                                                                         float bottom = out.at<float>(j, z);
01211                                                                                         float slope = (bottom-top)/float(factor);
01212                                                                                         for(int d=j-factor+1; d<j; ++d)
01213                                                                                         {
01214                                                                                                 out.at<float>(d, z) = top+(slope*float(d-(j-factor)));
01215                                                                                         }
01216                                                                                 }
01217                                                                         }
01218                                                                         else
01219                                                                         {
01220                                                                                 for(int z=i-factor; z<=i; ++z)
01221                                                                                 {
01222                                                                                         float top = out.at<unsigned short>(j-factor, z);
01223                                                                                         float bottom = out.at<unsigned short>(j, z);
01224                                                                                         float slope = (bottom-top)/float(factor);
01225                                                                                         for(int d=j-factor+1; d<j; ++d)
01226                                                                                         {
01227                                                                                                 out.at<unsigned short>(d, z) = (unsigned short)(top+(slope*float(d-(j-factor))));
01228                                                                                         }
01229                                                                                 }
01230                                                                         }
01231                                                                 }
01232                                                         }
01233                                                 }
01234                                         }
01235                                 }
01236                         }
01237                         else
01238                         {
01239                                 cv::resize(image, out, cv::Size(), float(factor), float(factor));
01240                         }
01241                 }
01242                 else
01243                 {
01244                         out = image;
01245                 }
01246         }
01247         return out;
01248 }
01249 
01250 // Registration Depth to RGB (return registered depth image)
01251 cv::Mat registerDepth(
01252                 const cv::Mat & depth,
01253                 const cv::Mat & depthK,
01254                 const cv::Mat & colorK,
01255                 const rtabmap::Transform & transform)
01256 {
01257         UASSERT(!transform.isNull());
01258         UASSERT(!depth.empty());
01259         UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1); // mm or m
01260         UASSERT(depthK.type() == CV_64FC1 && depthK.cols == 3 && depthK.cols == 3);
01261         UASSERT(colorK.type() == CV_64FC1 && colorK.cols == 3 && colorK.cols == 3);
01262 
01263         float fx = depthK.at<double>(0,0);
01264         float fy = depthK.at<double>(1,1);
01265         float cx = depthK.at<double>(0,2);
01266         float cy = depthK.at<double>(1,2);
01267 
01268         float rfx = colorK.at<double>(0,0);
01269         float rfy = colorK.at<double>(1,1);
01270         float rcx = colorK.at<double>(0,2);
01271         float rcy = colorK.at<double>(1,2);
01272 
01273         Eigen::Affine3f proj = transform.toEigen3f();
01274         Eigen::Vector4f P4,P3;
01275         P4[3] = 1;
01276         cv::Mat registered = cv::Mat::zeros(depth.rows, depth.cols, depth.type());
01277 
01278         bool depthInMM = depth.type() == CV_16UC1;
01279         for(int y=0; y<depth.rows; ++y)
01280         {
01281                 for(int x=0; x<depth.cols; ++x)
01282                 {
01283                         //filtering
01284                         float dz = depthInMM?float(depth.at<unsigned short>(y,x))*0.001f:depth.at<float>(y,x); // put in meter for projection
01285                         if(dz>=0.0f)
01286                         {
01287                                 // Project to 3D
01288                                 P4[0] = (x - cx) * dz / fx; // Optimization: we could have (x-cx)/fx in a lookup table
01289                                 P4[1] = (y - cy) * dz / fy; // Optimization: we could have (y-cy)/fy in a lookup table
01290                                 P4[2] = dz;
01291 
01292                                 P3 = proj * P4;
01293                                 float z = P3[2];
01294                                 float invZ = 1.0f/z;
01295                                 int dx = (rfx*P3[0])*invZ + rcx;
01296                                 int dy = (rfy*P3[1])*invZ + rcy;
01297 
01298                                 if(uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
01299                                 {
01300                                         if(depthInMM)
01301                                         {
01302                                                 unsigned short z16 = z * 1000; //mm
01303                                                 unsigned short &zReg = registered.at<unsigned short>(dy, dx);
01304                                                 if(zReg == 0 || z16 < zReg)
01305                                                 {
01306                                                         zReg = z16;
01307                                                 }
01308                                         }
01309                                         else
01310                                         {
01311                                                 float &zReg = registered.at<float>(dy, dx);
01312                                                 if(zReg == 0 || z < zReg)
01313                                                 {
01314                                                         zReg = z;
01315                                                 }
01316                                         }
01317                                 }
01318                         }
01319                 }
01320         }
01321         return registered;
01322 }
01323 
01324 cv::Mat fillDepthHoles(const cv::Mat & registeredDepth, int maximumHoleSize, float errorRatio)
01325 {
01326         UASSERT(registeredDepth.type() == CV_16UC1);
01327         UASSERT(maximumHoleSize > 0);
01328         cv::Mat output = registeredDepth.clone();
01329         for(int y=0; y<registeredDepth.rows-2; ++y)
01330         {
01331                 for(int x=0; x<registeredDepth.cols-2; ++x)
01332                 {
01333                         float a = registeredDepth.at<unsigned short>(y, x);
01334                         float bRight = registeredDepth.at<unsigned short>(y, x+1);
01335                         float bDown = registeredDepth.at<unsigned short>(y+1, x);
01336 
01337                         if(a > 0.0f && (bRight == 0.0f || bDown == 0.0f))
01338                         {
01339                                 bool horizontalSet = bRight != 0.0f;
01340                                 bool verticalSet = bDown != 0.0f;
01341                                 int stepX = 0;
01342                                 for(int h=1; h<=maximumHoleSize && (!horizontalSet || !verticalSet); ++h)
01343                                 {
01344                                         // horizontal
01345                                         if(!horizontalSet)
01346                                         {
01347                                                 if(x+1+h >= registeredDepth.cols)
01348                                                 {
01349                                                         horizontalSet = true;
01350                                                 }
01351                                                 else
01352                                                 {
01353                                                         float c = registeredDepth.at<unsigned short>(y, x+1+h);
01354                                                         if(c == 0)
01355                                                         {
01356                                                                 // ignore this size
01357                                                         }
01358                                                         else
01359                                                         {
01360                                                                 // fill hole
01361                                                                 float depthError = errorRatio*float(a+c)/2.0f;
01362                                                                 if(fabs(a-c) <= depthError)
01363                                                                 {
01364                                                                         //linear interpolation
01365                                                                         float slope = (c-a)/float(h+1);
01366                                                                         for(int z=x+1; z<x+1+h; ++z)
01367                                                                         {
01368                                                                                 if(output.at<unsigned short>(y, z) == 0)
01369                                                                                 {
01370                                                                                         output.at<unsigned short>(y, z) = (unsigned short)(a+(slope*float(z-x)));
01371                                                                                 }
01372                                                                                 else
01373                                                                                 {
01374                                                                                         // average with the previously set value
01375                                                                                         output.at<unsigned short>(y, z) = (output.at<unsigned short>(y, z)+(unsigned short)(a+(slope*float(z-x))))/2;
01376                                                                                 }
01377                                                                         }
01378                                                                 }
01379                                                                 horizontalSet = true;
01380                                                                 stepX = h;
01381                                                         }
01382                                                 }
01383                                         }
01384 
01385                                         // vertical
01386                                         if(!verticalSet)
01387                                         {
01388                                                 if(y+1+h >= registeredDepth.rows)
01389                                                 {
01390                                                         verticalSet = true;
01391                                                 }
01392                                                 else
01393                                                 {
01394                                                         float c = registeredDepth.at<unsigned short>(y+1+h, x);
01395                                                         if(c == 0)
01396                                                         {
01397                                                                 // ignore this size
01398                                                         }
01399                                                         else
01400                                                         {
01401                                                                 // fill hole
01402                                                                 float depthError = errorRatio*float(a+c)/2.0f;
01403                                                                 if(fabs(a-c) <= depthError)
01404                                                                 {
01405                                                                         //linear interpolation
01406                                                                         float slope = (c-a)/float(h+1);
01407                                                                         for(int z=y+1; z<y+1+h; ++z)
01408                                                                         {
01409                                                                                 if(output.at<unsigned short>(z, x) == 0)
01410                                                                                 {
01411                                                                                         output.at<unsigned short>(z, x) = (unsigned short)(a+(slope*float(z-y)));
01412                                                                                 }
01413                                                                                 else
01414                                                                                 {
01415                                                                                         // average with the previously set value
01416                                                                                         output.at<unsigned short>(z, x) = (output.at<unsigned short>(z, x)+(unsigned short)(a+(slope*float(z-y))))/2;
01417                                                                                 }
01418                                                                         }
01419                                                                 }
01420                                                                 verticalSet = true;
01421                                                         }
01422                                                 }
01423                                         }
01424                                 }
01425                                 x+=stepX;
01426                         }
01427                 }
01428         }
01429         return output;
01430 }
01431 
01432 void fillRegisteredDepthHoles(cv::Mat & registeredDepth, bool vertical, bool horizontal, bool fillDoubleHoles)
01433 {
01434         UASSERT(registeredDepth.type() == CV_16UC1);
01435         int margin = fillDoubleHoles?2:1;
01436         for(int x=1; x<registeredDepth.cols-margin; ++x)
01437         {
01438                 for(int y=1; y<registeredDepth.rows-margin; ++y)
01439                 {
01440                         unsigned short & b = registeredDepth.at<unsigned short>(y, x);
01441                         bool set = false;
01442                         if(vertical)
01443                         {
01444                                 const unsigned short & a = registeredDepth.at<unsigned short>(y-1, x);
01445                                 unsigned short & c = registeredDepth.at<unsigned short>(y+1, x);
01446                                 if(a && c)
01447                                 {
01448                                         unsigned short error = 0.01*((a+c)/2);
01449                                         if(((b == 0 && a && c) || (b > a+error && b > c+error)) &&
01450                                                 (a>c?a-c<=error:c-a<=error))
01451                                         {
01452                                                 b = (a+c)/2;
01453                                                 set = true;
01454                                                 if(!horizontal)
01455                                                 {
01456                                                         ++y;
01457                                                 }
01458                                         }
01459                                 }
01460                                 if(!set && fillDoubleHoles)
01461                                 {
01462                                         const unsigned short & d = registeredDepth.at<unsigned short>(y+2, x);
01463                                         if(a && d && (b==0 || c==0))
01464                                         {
01465                                                 unsigned short error = 0.01*((a+d)/2);
01466                                                 if(((b == 0 && a && d) || (b > a+error && b > d+error)) &&
01467                                                    ((c == 0 && a && d) || (c > a+error && c > d+error)) &&
01468                                                         (a>d?a-d<=error:d-a<=error))
01469                                                 {
01470                                                         if(a>d)
01471                                                         {
01472                                                                 unsigned short tmp = (a-d)/4;
01473                                                                 b = d + tmp;
01474                                                                 c = d + 3*tmp;
01475                                                         }
01476                                                         else
01477                                                         {
01478                                                                 unsigned short tmp = (d-a)/4;
01479                                                                 b = a + tmp;
01480                                                                 c = a + 3*tmp;
01481                                                         }
01482                                                         set = true;
01483                                                         if(!horizontal)
01484                                                         {
01485                                                                 y+=2;
01486                                                         }
01487                                                 }
01488                                         }
01489                                 }
01490                         }
01491                         if(!set && horizontal)
01492                         {
01493                                 const unsigned short & a = registeredDepth.at<unsigned short>(y, x-1);
01494                                 unsigned short & c = registeredDepth.at<unsigned short>(y, x+1);
01495                                 if(a && c)
01496                                 {
01497                                         unsigned short error = 0.01*((a+c)/2);
01498                                         if(((b == 0 && a && c) || (b > a+error && b > c+error)) &&
01499                                                 (a>c?a-c<=error:c-a<=error))
01500                                         {
01501                                                 b = (a+c)/2;
01502                                                 set = true;
01503                                         }
01504                                 }
01505                                 if(!set && fillDoubleHoles)
01506                                 {
01507                                         const unsigned short & d = registeredDepth.at<unsigned short>(y, x+2);
01508                                         if(a && d && (b==0 || c==0))
01509                                         {
01510                                                 unsigned short error = 0.01*((a+d)/2);
01511                                                 if(((b == 0 && a && d) || (b > a+error && b > d+error)) &&
01512                                                    ((c == 0 && a && d) || (c > a+error && c > d+error)) &&
01513                                                         (a>d?a-d<=error:d-a<=error))
01514                                                 {
01515                                                         if(a>d)
01516                                                         {
01517                                                                 unsigned short tmp = (a-d)/4;
01518                                                                 b = d + tmp;
01519                                                                 c = d + 3*tmp;
01520                                                         }
01521                                                         else
01522                                                         {
01523                                                                 unsigned short tmp = (d-a)/4;
01524                                                                 b = a + tmp;
01525                                                                 c = a + 3*tmp;
01526                                                         }
01527                                                 }
01528                                         }
01529                                 }
01530                         }
01531                 }
01532         }
01533 }
01534 
01535 }
01536 
01537 }

rtabmap
Author(s): Mathieu Labbe
autogenerated on Sat Jul 23 2016 11:44:28