i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-08 12:30:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated stereo block matching approach (when Stereo/OpticalFlow=false)

Browse Source
This commit is contained in:
matlabbe
2018-11-13 14:25:21 -05:00
parent 227f8c4f86
commit 9efd7c14fc
2 changed files with 67 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/util2d.cpp
+44 -40
View File
@@ -137,6 +137,7 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
UDEBUG("maxDisparity=%f", maxDisparityF);
UDEBUG("iterations=%d", iterations);
UDEBUG("ssdApproach=%d", ssdApproach?1:0);
UASSERT(minDisparityF >= 0.0f && minDisparityF <= maxDisparityF);
// window should be odd
if(winSize.width%2 == 0)
@@ -175,7 +176,7 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
int tmpMinDisparity = minDisparity;
int tmpMaxDisparity = maxDisparity;
int iterations = 0;
int iterationsDone = 0;
for(int level=maxLevel; level>=0; --level)
{
UASSERT(level < (int)leftPyramid.size());
@@ -194,62 +195,65 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
cv::Mat windowLeft(leftPyramid[level],
cv::Range(center.y-halfWin.height,center.y+halfWin.height+1),
cv::Range(center.x-halfWin.width,center.x+halfWin.width+1));
int minCol = center.x+localMaxDisparity-halfWin.width-1;
int minCol = center.x+localMaxDisparity-halfWin.width;
if(minCol < 0)
{
localMaxDisparity -= minCol;
}
int maxCol = center.x+localMinDisparity+halfWin.width+1;
if(maxCol >= leftPyramid[level].cols)
if(localMinDisparity > localMaxDisparity)
{
localMinDisparity += maxCol-leftPyramid[level].cols-1;
}
int length = localMinDisparity-localMaxDisparity+1;
std::vector<float> scores = std::vector<float>(length, 0.0f);
if(localMinDisparity < localMaxDisparity)
{
localMaxDisparity = localMinDisparity;
}
int length = localMinDisparity-localMaxDisparity+1;
std::vector<float> scores = std::vector<float>(length, 0.0f);
for(int d=localMinDisparity; d>localMaxDisparity; --d)
{
++iterations;
cv::Mat windowRight(rightPyramid[level],
cv::Range(center.y-halfWin.height,center.y+halfWin.height+1),
cv::Range(center.x+d-halfWin.width,center.x+d+halfWin.width+1));
scores[oi] = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
if(scores[oi] > 0 && (bestScore < 0.0f || scores[oi] < bestScore))
for(int d=localMinDisparity; d>localMaxDisparity; --d)
{
bestScoreIndex = oi;
bestScore = scores[oi];
}
++oi;
}
if(bestScoreIndex>=0)
{
if(level>0)
{
tmpMaxDisparity = tmpMinDisparity+(bestScoreIndex+1)*(1<<level);
tmpMaxDisparity+=tmpMaxDisparity%level;
if(tmpMaxDisparity > maxDisparity)
++iterationsDone;
cv::Mat windowRight(rightPyramid[level],
cv::Range(center.y-halfWin.height,center.y+halfWin.height+1),
cv::Range(center.x+d-halfWin.width,center.x+d+halfWin.width+1));
scores[oi] = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
if(scores[oi] > 0 && (bestScore < 0.0f || scores[oi] < bestScore))
{
tmpMaxDisparity = maxDisparity;
bestScoreIndex = oi;
bestScore = scores[oi];
}
tmpMinDisparity = tmpMinDisparity+(bestScoreIndex-1)*(1<<level);
tmpMinDisparity -= tmpMinDisparity%level;
if(tmpMinDisparity < minDisparity)
++oi;
}
if(oi>1)
{
float m = uMean(scores);
float st = sqrt(uVariance(scores, m));
if(bestScore > st)
{
tmpMinDisparity = minDisparity;
bestScoreIndex = -1;
}
}
if(bestScoreIndex>=0)
{
if(bestScoreIndex>=0 && level>0)
{
tmpMaxDisparity = tmpMinDisparity+(bestScoreIndex+1)*(1<<level);
tmpMaxDisparity+=tmpMaxDisparity%level;
if(tmpMaxDisparity > maxDisparity)
{
tmpMaxDisparity = maxDisparity;
}
tmpMinDisparity = tmpMinDisparity+(bestScoreIndex-1)*(1<<level);
tmpMinDisparity -= tmpMinDisparity%level;
if(tmpMinDisparity < minDisparity)
{
tmpMinDisparity = minDisparity;
}
}
}
}
}
}
disparityTime+=timer.ticks();
totalIterations+=iterations;
totalIterations+=iterationsDone;
if(bestScoreIndex>=0)
{
tools/StereoEval/main.cpp
+23 -17
View File
@@ -304,7 +304,6 @@ int main(int argc, char * argv[])
int inliers = 0;
int subInliers = 0;
int badInliers = 0;
int outliers = 0;
float sumInliers = 0.0f;
float sumSubInliers = 0.0f;
int goodRejected = 0;
@@ -366,26 +365,37 @@ int main(int argc, char * argv[])
cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
cv::Scalar( 0, 0, 255 ));
++outliers;
++badInliers;
//UDEBUG("should be rejected: %d", i);
}
}
else
{
++badInliers;
}
}
else if(mask.at<cv::Vec3b>(int(rightCorners[i].y), int(leftCorners[i].x))[0] == 255 &&
rightCorners[i].x > 0.0f)
{
float d = leftCorners[i].x - rightCorners[i].x;
cv::line(left,
leftCorners[i],
cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
cv::Scalar( 0, 255, 255 ));
if(fabs(d-gt) < 1.0f)
{
cv::line(left,
leftCorners[i],
cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
cv::Scalar( 0, 255, 255 ));
cv::line(left,
cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
cv::Scalar( 0, 0, 255 ));
++goodRejected;
//UDEBUG("should not be rejected: %d", i);
cv::line(left,
cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
cv::Scalar( 0, 0, 255 ));
++goodRejected;
//UDEBUG("should not be rejected: %d", i);
}
else
{
++badRejected;
}
}
else
{
@@ -394,15 +404,11 @@ int main(int argc, char * argv[])
}
}
UINFO("inliers=%d (%d%%) subInliers=%d (%d%%) bad inliers=%d (%d%%) bad accepted=%d (%d%%) good rejected=%d (%d%%) bad rejected=%d (%d%%)",
UINFO("good accepted=%d (%d%%) bad accepted=%d (%d%%) good rejected=%d (%d%%) bad rejected=%d (%d%%)",
inliers,
(inliers*100)/leftCorners.size(),
subInliers,
(subInliers*100)/leftCorners.size(),
badInliers,
(badInliers*100)/leftCorners.size(),
outliers,
(outliers*100)/leftCorners.size(),
goodRejected,
(goodRejected*100)/leftCorners.size(),
badRejected,