Updated 32F disparity matrix handling

modules/ximgproc/src/disparity_filters.cpp

@@ -219,16 +219,31 @@ Ptr<DisparityWLSFilterImpl> DisparityWLSFilterImpl::create(bool _use_confidence,
 
 void DisparityWLSFilterImpl::filter(InputArray disparity_map_left, InputArray left_view, OutputArray filtered_disparity_map, InputArray disparity_map_right, Rect ROI, InputArray)
 {
-    Mat left = disparity_map_left.getMat();
-    if (left.type() == CV_32F){
-        filter_(disparity_map_left, left_view, filtered_disparity_map, disparity_map_right, ROI);
-    } else {
-        Mat left_c, right_c, filt_disp;
-        left.convertTo(left_c, CV_32F);
-        if(!disparity_map_right.empty())
-            disparity_map_right.getMat().convertTo(right_c, CV_32F);
-        filter_(left_c, left_view, filt_disp, right_c, ROI);
-        filt_disp.convertTo(filtered_disparity_map, CV_32F);
+    CV_Assert(!disparity_map_left.empty() && (disparity_map_left.channels() == 1));
+    CV_Assert(!left_view.empty() && (left_view.depth() == CV_8U) && (left_view.channels() == 3 || left_view.channels() == 1));
+    Mat left, right, filt_disp;
+
+    if (disparity_map_left.depth() != CV_32F)
+    {
+        disparity_map_left.getMat().convertTo(left, CV_32F);
+    }
+    else
+    {
+        left = disparity_map_left.getMat();
+        filt_disp = filtered_disparity_map.getMat();
+    }
+
+    if (!disparity_map_right.empty() && use_confidence)
+    {
+        if(disparity_map_right.depth() != CV_32F)
+            disparity_map_right.getMat().convertTo(right, CV_32F);
+        else
+            right = disparity_map_right.getMat();
+    }
+
+    filter_(left, left_view, filt_disp, right, ROI);
+    if (disparity_map_left.depth() != CV_32F){
+        filt_disp.convertTo(filtered_disparity_map, disparity_map_left.depth());
     }
 }
 
@@ -257,16 +272,17 @@ void DisparityWLSFilterImpl::filter_(InputArray disparity_map_left, InputArray l
     {
         Mat disp_full_size = disparity_map_left.getMat();
         Mat src_full_size = left_view.getMat();
-        if(disp_full_size.size!=src_full_size.size){
+        if(disp_full_size.size!=src_full_size.size)
+        {
             float x_ratio = src_full_size.cols/(float)disp_full_size.cols;
             float y_ratio = src_full_size.rows/(float)disp_full_size.rows;
             resize(disp_full_size,disp_full_size,src_full_size.size());
             disp_full_size = disp_full_size*x_ratio;
             ROI = Rect((int)(valid_disp_ROI.x*x_ratio),    (int)(valid_disp_ROI.y*y_ratio),
                        (int)(valid_disp_ROI.width*x_ratio),(int)(valid_disp_ROI.height*y_ratio));
-        } else {
-            ROI = valid_disp_ROI;
         }
+        else
+            ROI = valid_disp_ROI;
 
         disp = Mat(disp_full_size,ROI);
         src  = Mat(src_full_size ,ROI);
@@ -348,7 +364,7 @@ void DisparityWLSFilterImpl::ComputeDiscontinuityAwareLRC_ParBody::operator() (c
         int right_end = right_ROI.x+right_ROI.width;
         for(int j=j_start;j<j_end;j++)
         {
-                right_idx = j-((int)row_left[j]);
+            right_idx = j-(((int)row_left[j])>>4);
             if( right_idx>=right_ROI.x && right_idx<right_end)
             {
                 if(abs(row_left[j] + row_right[right_idx])< thresh)
@@ -519,21 +535,13 @@ double computeMSE(InputArray GT, InputArray src, Rect ROI)
     CV_Assert( !GT.empty()  && (GT.depth()  == CV_16S || GT.depth()  == CV_32F) && (GT.channels()  == 1) );
     CV_Assert( !src.empty() && (src.depth() == CV_16S || src.depth()  == CV_32F) && (src.channels() == 1) );
     CV_Assert( src.rows() == GT.rows() && src.cols() == GT.cols() );
-    double res = 0;
     Mat GT_ROI (GT.getMat(), ROI);
     Mat src_ROI(src.getMat(),ROI);
-    int cnt=0;
-    for(int i=0;i<src_ROI.rows;i++)
-        for(int j=0;j<src_ROI.cols;j++)
-        {
-            if(GT_ROI.at<short>(i,j)!=UNKNOWN_DISPARITY)
-            {
-                res += (GT_ROI.at<short>(i,j) - src_ROI.at<short>(i,j))*(GT_ROI.at<short>(i,j) - src_ROI.at<short>(i,j));
-                cnt++;
-            }
-        }
-    res /= cnt*256;
-    return res;
+
+    Mat tmp, dtmp, gt_mask = (GT_ROI == UNKNOWN_DISPARITY);
+    absdiff(GT_ROI, src_ROI, tmp); tmp.setTo(0, gt_mask); multiply(tmp, tmp, tmp);
+    tmp.convertTo(dtmp, CV_64FC1);
+    return sum(dtmp)[0] / ((gt_mask.total() - countNonZero(gt_mask))*256);
 }
 
 double computeBadPixelPercent(InputArray GT, InputArray src, Rect ROI, int thresh)
@@ -541,21 +549,13 @@ double computeBadPixelPercent(InputArray GT, InputArray src, Rect ROI, int thres
     CV_Assert( !GT.empty()  && (GT.depth()  == CV_16S || GT.depth()  == CV_32F) && (GT.channels()  == 1) );
     CV_Assert( !src.empty() && (src.depth() == CV_16S || src.depth()  == CV_32F) && (src.channels() == 1) );
     CV_Assert( src.rows() == GT.rows() && src.cols() == GT.cols() );
-    int bad_pixel_num = 0;
     Mat GT_ROI (GT.getMat(), ROI);
     Mat src_ROI(src.getMat(),ROI);
-    int cnt=0;
-    for(int i=0;i<src_ROI.rows;i++)
-        for(int j=0;j<src_ROI.cols;j++)
-        {
-            if(GT_ROI.at<short>(i,j)!=UNKNOWN_DISPARITY)
-            {
-                if( abs(GT_ROI.at<short>(i,j) - src_ROI.at<short>(i,j))>=thresh )
-                    bad_pixel_num++;
-                cnt++;
-            }
-        }
-    return (100.0*bad_pixel_num)/cnt;
+
+    Mat tmp, gt_mask = (GT_ROI == UNKNOWN_DISPARITY);
+    absdiff(GT_ROI, src_ROI, tmp); tmp.setTo(0, gt_mask);
+    cv::threshold(tmp, tmp, thresh - 1, 1, THRESH_BINARY);
+    return (100.0 * countNonZero(tmp)) / (gt_mask.total() - countNonZero(gt_mask));
 }
 
 void getDisparityVis(InputArray src,OutputArray dst,double scale)
@@ -565,14 +565,8 @@ void getDisparityVis(InputArray src,OutputArray dst,double scale)
     dst.create(srcMat.rows,srcMat.cols,CV_8UC1);
     Mat& dstMat = dst.getMatRef();
 
-    for(int i=0;i<dstMat.rows;i++)
-        for(int j=0;j<dstMat.cols;j++)
-        {
-            if(srcMat.at<short>(i,j)==UNKNOWN_DISPARITY)
-                dstMat.at<unsigned char>(i,j) = 0;
-            else
-                dstMat.at<unsigned char>(i,j) = saturate_cast<unsigned char>(scale*srcMat.at<short>(i,j)/16.0);
-        }
+    srcMat.convertTo(dstMat, CV_8UC1, scale / 16.0);
+    dstMat &= (srcMat != UNKNOWN_DISPARITY);
 }
 
 }