Merge pull request #239 from sbokov/disparityFiltering

Interface and demo for disparity filtering, basic WLS implementation

modules/ximgproc/CMakeLists.txt

@@ -1,5 +1,5 @@
 set(the_description "Extended image processing module. It includes edge-aware filters and etc.")
 set(OPENCV_MODULE_IS_PART_OF_WORLD OFF)
-ocv_define_module(ximgproc opencv_imgproc opencv_core opencv_highgui WRAP python)
+ocv_define_module(ximgproc opencv_imgproc opencv_core opencv_highgui opencv_calib3d WRAP python)
 
 target_link_libraries(opencv_ximgproc)

modules/ximgproc/include/opencv2/ximgproc/disparity_filter.hpp

@@ -0,0 +1,92 @@
+/*
+ *  By downloading, copying, installing or using the software you agree to this license.
+ *  If you do not agree to this license, do not download, install,
+ *  copy or use the software.
+ *
+ *
+ *  License Agreement
+ *  For Open Source Computer Vision Library
+ *  (3 - clause BSD License)
+ *
+ *  Redistribution and use in source and binary forms, with or without modification,
+ *  are permitted provided that the following conditions are met :
+ *
+ *  *Redistributions of source code must retain the above copyright notice,
+ *  this list of conditions and the following disclaimer.
+ *
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *  this list of conditions and the following disclaimer in the documentation
+ *  and / or other materials provided with the distribution.
+ *
+ *  * Neither the names of the copyright holders nor the names of the contributors
+ *  may be used to endorse or promote products derived from this software
+ *  without specific prior written permission.
+ *
+ *  This software is provided by the copyright holders and contributors "as is" and
+ *  any express or implied warranties, including, but not limited to, the implied
+ *  warranties of merchantability and fitness for a particular purpose are disclaimed.
+ *  In no event shall copyright holders or contributors be liable for any direct,
+ *  indirect, incidental, special, exemplary, or consequential damages
+ *  (including, but not limited to, procurement of substitute goods or services;
+ *  loss of use, data, or profits; or business interruption) however caused
+ *  and on any theory of liability, whether in contract, strict liability,
+ *  or tort(including negligence or otherwise) arising in any way out of
+ *  the use of this software, even if advised of the possibility of such damage.
+ */
+
+#ifndef __OPENCV_DISPARITYFILTER_HPP__
+#define __OPENCV_DISPARITYFILTER_HPP__
+#ifdef __cplusplus
+
+#include <opencv2/core.hpp>
+
+namespace cv {
+namespace ximgproc {
+
+class CV_EXPORTS_W DisparityFilter : public Algorithm
+{
+public:
+    CV_WRAP virtual void filter(InputArray disparity_map, InputArray left_view, OutputArray filtered_disparity_map,Rect ROI) = 0;
+};
+
+/////////////////////////////////////////////////////////////////////////////////////////////////
+
+class CV_EXPORTS_W DisparityDTFilter : public DisparityFilter
+{
+public:
+    CV_WRAP virtual double getSigmaSpatial() = 0;
+    CV_WRAP virtual void setSigmaSpatial(double _sigmaSpatial) = 0;
+    CV_WRAP virtual double getSigmaColor() = 0;
+    CV_WRAP virtual void setSigmaColor(double _sigmaColor) = 0;
+};
+
+CV_EXPORTS_W
+Ptr<DisparityDTFilter> createDisparityDTFilter();
+
+class CV_EXPORTS_W DisparityGuidedFilter : public DisparityFilter
+{
+public:
+    CV_WRAP virtual double getEps() = 0;
+    CV_WRAP virtual void setEps(double _eps) = 0;
+    CV_WRAP virtual int getRadius() = 0;
+    CV_WRAP virtual void setRadius(int _radius) = 0;
+};
+
+CV_EXPORTS_W
+Ptr<DisparityGuidedFilter> createDisparityGuidedFilter();
+
+class CV_EXPORTS_W DisparityWLSFilter : public DisparityFilter
+{
+    CV_WRAP virtual double getLambda() = 0;
+    CV_WRAP virtual void setLambda(double _lambda) = 0;
+    CV_WRAP virtual double getSigmaColor() = 0;
+    CV_WRAP virtual void setSigmaColor(double _sigma_color) = 0;
+};
+
+CV_EXPORTS_W
+Ptr<DisparityWLSFilter> createDisparityWLSFilter();
+
+}
+}
+#endif
+#endif

modules/ximgproc/include/opencv2/ximgproc/edge_filter.hpp

@@ -318,6 +318,17 @@ void jointBilateralFilter(InputArray joint, InputArray src, OutputArray dst, int
 
 //! @}
 
+class CV_EXPORTS_W WeightedLeastSquaresFilter : public Algorithm
+{
+public:
+    CV_WRAP virtual void filter(InputArray src, OutputArray dst) = 0;
+    virtual ~WeightedLeastSquaresFilter();
+};
+
+CV_EXPORTS_W Ptr<WeightedLeastSquaresFilter> createWeightedLeastSquaresFilter(InputArray guide, double lambda, double sigma_color, int num_iter=3);
+
+CV_EXPORTS_W void weightedLeastSquaresFilter(InputArray guide, InputArray src, OutputArray dst, double lambda, double sigma_color, int num_iter=3);
+
 }
 }
 #endif

modules/ximgproc/samples/disparity_filtering.cpp

@@ -0,0 +1,258 @@
+#include "opencv2/calib3d.hpp"
+#include "opencv2/imgproc.hpp"
+#include "opencv2/imgcodecs.hpp"
+#include "opencv2/highgui.hpp"
+#include "opencv2/core/utility.hpp"
+#include "opencv2/ximgproc/disparity_filter.hpp"
+#include <stdio.h>
+#include <string>
+#include <vector>
+#include <map>
+
+#if defined(_WIN32)
+#include <direct.h>
+#else 
+#include <sys/stat.h>
+#endif
+
+using namespace cv;
+using namespace cv::ximgproc;
+using namespace std;
+
+#define UNKNOWN_DISPARITY 16320
+ 
+static void print_help()
+{
+    printf("\nDemo for disparity filtering, evaluating speed and performance of different filters\n");
+    printf("\nUsage: disparity_filtering.exe <path_to_dataset_folder> <path_to_results_folder>\n");
+}
+
+struct dataset_entry
+{
+    string name;
+    string dataset_folder;
+    string left_file,right_file,GT_file;
+    dataset_entry(string _dataset_folder): dataset_folder(_dataset_folder){}
+    void readEntry(Mat& dst_left,Mat& dst_right,Mat& dst_GT)
+    {
+        dst_left  = imread(dataset_folder+"/"+left_file,  IMREAD_COLOR);
+        dst_right = imread(dataset_folder+"/"+right_file, IMREAD_COLOR);
+        Mat raw_disp = imread(dataset_folder+"/"+GT_file, IMREAD_COLOR);
+        dst_GT = Mat(raw_disp.rows,raw_disp.cols,CV_16S);
+        for(int i=0;i<raw_disp.rows;i++)
+            for(int j=0;j<raw_disp.cols;j++)
+            {
+                Vec3b bgrPixel = raw_disp.at<Vec3b>(i, j);
+                dst_GT.at<short>(i,j) = 64*bgrPixel.val[2]+bgrPixel.val[1]/4; //16-multiplied disparity
+            }
+    }
+};
+
+struct config
+{
+    Ptr<StereoMatcher> matcher_instance;
+    Ptr<DisparityFilter> filter_instance;
+    config(Ptr<StereoMatcher> _matcher_instance,Ptr<DisparityFilter> _filter_instance)
+    {
+        matcher_instance = _matcher_instance;
+        filter_instance = _filter_instance;
+    }
+    config() {}
+};
+
+void operator>>(const FileNode& node,dataset_entry& entry);
+double computeMSE(Mat& GT, Mat& src, Rect ROI);
+double computeBadPixelPercent(Mat& GT, Mat& src, Rect ROI, int thresh=24/*1.5 pixels*/);
+void getDisparityVis(Mat& disparity_map,Mat& dst);
+Rect computeROI(Size2i src_sz, Ptr<StereoMatcher> matcher_instance);
+void setConfigsForTesting(map<string,config>& cfgs);
+void CreateDir(string path);
+
+int main(int argc, char** argv)
+{
+    if(argc < 3)
+    {
+        print_help();
+        return 0;
+    }
+    string dataset_folder(argv[1]);
+    string res_folder(argv[2]);
+
+    map<string,config> configs_for_testing;
+    setConfigsForTesting(configs_for_testing);
+    CreateDir(res_folder);
+
+    for (map<string,config>::iterator cfg = configs_for_testing.begin(); cfg != configs_for_testing.end(); cfg++)
+    {
+        string vis_folder = res_folder+"/vis_"+cfg->first;
+        CreateDir(vis_folder);
+
+        string cfg_file_name = res_folder+"/"+cfg->first+"_res.csv";
+        FILE* cur_cfg_res_file = fopen(cfg_file_name.c_str(),"w");
+        fprintf(cur_cfg_res_file,"Name,MSE,MSE after postfiltering,Percent bad,Percent bad after postfiltering,Matcher Execution Time(s),Filter Execution Time(s)\n");
+
+        printf("Processing configuration: %s\n",cfg->first.c_str());
+
+        FileStorage fs(dataset_folder + "/_dataset.xml", FileStorage::READ);
+        FileNode n = fs["data_set"];
+        double MSE_pre,percent_pre,MSE_post,percent_post,matching_time,filtering_time;
+        double average_MSE_pre=0,average_percent_pre=0,average_MSE_post=0,
+            average_percent_post=0,average_matching_time=0,average_filtering_time=0;
+        int cnt = 0;
+        for (FileNodeIterator it = n.begin(); it != n.end(); it++)
+        {
+            dataset_entry entry(dataset_folder);
+            (*it)>>entry;
+            printf("%s ",entry.name.c_str());
+            Mat left,right,GT;
+            entry.readEntry(left,right,GT);
+            Mat raw_disp;
+            Mat left_gray; cvtColor(left, left_gray, COLOR_BGR2GRAY );
+            Mat right_gray; cvtColor(right, right_gray, COLOR_BGR2GRAY );
+            matching_time = (double)getTickCount();
+            cfg->second.matcher_instance->compute(left_gray,right_gray,raw_disp);
+            matching_time = ((double)getTickCount() - matching_time)/getTickFrequency();
+
+            Rect ROI = computeROI(left.size(),cfg->second.matcher_instance);
+            Mat filtered_disp;
+            filtering_time = (double)getTickCount();
+            cfg->second.filter_instance->filter(raw_disp,left,filtered_disp,ROI);
+            filtering_time = ((double)getTickCount() - filtering_time)/getTickFrequency();
+
+
+            MSE_pre = computeMSE(GT,raw_disp,ROI);
+            percent_pre = computeBadPixelPercent(GT,raw_disp,ROI);
+            MSE_post = computeMSE(GT,filtered_disp,ROI);
+            percent_post = computeBadPixelPercent(GT,filtered_disp,ROI);
+
+            fprintf(cur_cfg_res_file,"%s,%.1f,%.1f,%.1f,%.1f,%.3f,%.3f\n",entry.name.c_str(),MSE_pre,MSE_post,
+                percent_pre,percent_post,matching_time,filtering_time);
+
+            average_matching_time+=matching_time; average_filtering_time+=filtering_time;
+            average_MSE_pre+=MSE_pre; average_percent_pre+=percent_pre;
+            average_MSE_post+=MSE_post; average_percent_post+=percent_post;
+            cnt++;
+
+            // dump visualizations:
+            imwrite(vis_folder + "/" + entry.name + "_left.png",left);
+            Mat GT_vis,raw_disp_vis,filtered_disp_vis;
+            getDisparityVis(GT,GT_vis);
+            getDisparityVis(raw_disp,raw_disp_vis);
+            getDisparityVis(filtered_disp,filtered_disp_vis);
+            imwrite(vis_folder + "/" + entry.name + "_disparity_GT.png",GT_vis);
+            imwrite(vis_folder + "/" + entry.name + "_disparity_raw.png",raw_disp_vis);
+            imwrite(vis_folder + "/" + entry.name + "_disparity_filtered.png",filtered_disp_vis);
+
+            printf("- Done\n");
+
+        }
+        fprintf(cur_cfg_res_file,"%s,%.1f,%.1f,%.1f,%.1f,%.3f,%.3f\n","average",average_MSE_pre/cnt,
+            average_MSE_post/cnt,average_percent_pre/cnt,average_percent_post/cnt,
+            average_matching_time/cnt,average_filtering_time/cnt);
+        fclose(cur_cfg_res_file);
+    }
+    return 0;
+}
+
+void operator>>(const FileNode& node,dataset_entry& entry) 
+{
+    node["name"] >> entry.name;
+    node["left_file"] >> entry.left_file;
+    node["right_file"] >> entry.right_file;
+    node["GT_file"] >> entry.GT_file;
+}
+
+double computeMSE(Mat& GT, Mat& src, Rect ROI)
+{
+    double res = 0;
+    Mat GT_ROI(GT,ROI);
+    Mat src_ROI(src,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;
+}
+
+double computeBadPixelPercent(Mat& GT, Mat& src, Rect ROI, int thresh)
+{
+    int bad_pixel_num = 0;
+    Mat GT_ROI(GT,ROI);
+    Mat src_ROI(src,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;
+}
+
+void getDisparityVis(Mat& disparity_map,Mat& dst)
+{
+    dst = Mat(disparity_map.rows,disparity_map.cols,CV_8UC3);
+    for(int i=0;i<dst.rows;i++)
+        for(int j=0;j<dst.cols;j++)
+        {
+            if(disparity_map.at<short>(i,j)==UNKNOWN_DISPARITY)
+                dst.at<Vec3b>(i,j) = Vec3b(0,0,0);
+            else
+                dst.at<Vec3b>(i,j) = Vec3b(saturate_cast<unsigned char>(disparity_map.at<short>(i,j)/8),
+                                           saturate_cast<unsigned char>(disparity_map.at<short>(i,j)/8),
+                                           saturate_cast<unsigned char>(disparity_map.at<short>(i,j)/8));
+        }
+}
+
+Rect computeROI(Size2i src_sz, Ptr<StereoMatcher> matcher_instance)
+{
+    int min_disparity = matcher_instance->getMinDisparity();
+    int num_disparities = matcher_instance->getNumDisparities();
+    int block_size = matcher_instance->getBlockSize();
+
+    int bs2 = block_size/2;
+    int minD = min_disparity, maxD = min_disparity + num_disparities - 1;
+
+    int xmin = maxD + bs2;
+    int xmax = src_sz.width - minD - bs2;
+    int ymin = bs2;
+    int ymax = src_sz.height - bs2;
+
+    Rect r(xmin, ymin, xmax - xmin, ymax - ymin);
+    return r;
+}
+
+void setConfigsForTesting(map<string,config>& cfgs)
+{
+    Ptr<StereoBM> stereobm_matcher = StereoBM::create(128,21); 
+    stereobm_matcher->setTextureThreshold(0); 
+    stereobm_matcher->setUniquenessRatio(0);
+
+    Ptr<DisparityFilter> wls_filter = createDisparityWLSFilter();
+    Ptr<DisparityFilter> dt_filter = createDisparityDTFilter();
+    Ptr<DisparityFilter> guided_filter = createDisparityGuidedFilter();
+
+    cfgs["stereobm_wls"] = config(stereobm_matcher,wls_filter);
+    cfgs["stereobm_dtf"] = config(stereobm_matcher,dt_filter);
+    cfgs["stereobm_gf"]  = config(stereobm_matcher,guided_filter);
+}
+
+void CreateDir(string path)
+{
+#if defined(_WIN32)
+    _mkdir(path.c_str());
+#else 
+    mkdir(path.c_str(), 0777);
+#endif
+}

modules/ximgproc/src/disparity_filters.cpp

@@ -0,0 +1,185 @@
+/*
+ *  By downloading, copying, installing or using the software you agree to this license.
+ *  If you do not agree to this license, do not download, install,
+ *  copy or use the software.
+ *  
+ *  
+ *  License Agreement
+ *  For Open Source Computer Vision Library
+ *  (3 - clause BSD License)
+ *  
+ *  Redistribution and use in source and binary forms, with or without modification,
+ *  are permitted provided that the following conditions are met :
+ *  
+ *  *Redistributions of source code must retain the above copyright notice,
+ *  this list of conditions and the following disclaimer.
+ *  
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *  this list of conditions and the following disclaimer in the documentation
+ *  and / or other materials provided with the distribution.
+ *  
+ *  * Neither the names of the copyright holders nor the names of the contributors
+ *  may be used to endorse or promote products derived from this software
+ *  without specific prior written permission.
+ *  
+ *  This software is provided by the copyright holders and contributors "as is" and
+ *  any express or implied warranties, including, but not limited to, the implied
+ *  warranties of merchantability and fitness for a particular purpose are disclaimed.
+ *  In no event shall copyright holders or contributors be liable for any direct,
+ *  indirect, incidental, special, exemplary, or consequential damages
+ *  (including, but not limited to, procurement of substitute goods or services;
+ *  loss of use, data, or profits; or business interruption) however caused
+ *  and on any theory of liability, whether in contract, strict liability,
+ *  or tort(including negligence or otherwise) arising in any way out of
+ *  the use of this software, even if advised of the possibility of such damage.
+ */
+
+#include "precomp.hpp"
+#include "opencv2/ximgproc/disparity_filter.hpp"
+#include <math.h>
+
+namespace cv {
+namespace ximgproc {
+
+void setROI(InputArray disparity_map, InputArray left_view, OutputArray filtered_disparity_map,
+            Mat& disp,Mat& src,Mat& dst,Rect ROI);
+
+class DisparityDTFilterImpl : public DisparityDTFilter
+{
+protected:
+    double sigmaSpatial,sigmaColor;
+    void init(double _sigmaSpatial, double _sigmaColor)
+    {
+        sigmaColor = _sigmaColor;
+        sigmaSpatial = _sigmaSpatial;
+    }
+public:
+    double getSigmaSpatial() {return sigmaSpatial;}
+    void setSigmaSpatial(double _sigmaSpatial) {sigmaSpatial = _sigmaSpatial;}
+    double getSigmaColor() {return sigmaColor;}
+    void setSigmaColor(double _sigmaColor) {sigmaColor = _sigmaColor;}
+
+    static Ptr<DisparityDTFilterImpl> create()
+    {
+        DisparityDTFilterImpl *dtf = new DisparityDTFilterImpl();
+        dtf->init(25.0,60.0); 
+        return Ptr<DisparityDTFilterImpl>(dtf);
+    }
+
+    void filter(InputArray disparity_map, InputArray left_view, OutputArray filtered_disparity_map,Rect ROI)
+    {
+        Mat disp,src,dst;
+        setROI(disparity_map,left_view,filtered_disparity_map,disp,src,dst,ROI);
+
+        Mat disp_32F,dst_32F;
+        disp.convertTo(disp_32F,CV_32F);
+        dtFilter(src,disp_32F,dst_32F,sigmaSpatial,sigmaColor);
+        dst_32F.convertTo(dst,CV_16S);
+    }
+};
+
+CV_EXPORTS_W
+Ptr<DisparityDTFilter> createDisparityDTFilter()
+{
+    return Ptr<DisparityDTFilter>(DisparityDTFilterImpl::create());
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+    
+class DisparityGuidedFilterImpl : public DisparityGuidedFilter
+{
+protected:
+    int radius;
+    double eps;
+    void init(int _radius, double _eps)
+    {
+        radius = _radius;
+        eps = _eps;
+    }
+public:
+    double getEps() {return eps;}
+    void setEps(double _eps) {eps = _eps;}
+    int getRadius() {return radius;}
+    void setRadius(int _radius) {radius = _radius;}
+
+    static Ptr<DisparityGuidedFilterImpl> create()
+    {
+        DisparityGuidedFilterImpl *gf = new DisparityGuidedFilterImpl();
+        gf->init(20,100.0);
+        return Ptr<DisparityGuidedFilterImpl>(gf);
+    }
+
+    void filter(InputArray disparity_map, InputArray left_view, OutputArray filtered_disparity_map,Rect ROI)
+    {
+        Mat disp,src,dst;
+        setROI(disparity_map,left_view,filtered_disparity_map,disp,src,dst,ROI);
+
+        Mat disp_32F,dst_32F;
+        disp.convertTo(disp_32F,CV_32F);
+        guidedFilter(src,disp_32F,dst_32F,radius,eps);
+        dst_32F.convertTo(dst,CV_16S);
+    }
+};
+
+CV_EXPORTS_W
+Ptr<DisparityGuidedFilter> createDisparityGuidedFilter()
+{
+    return Ptr<DisparityGuidedFilter>(DisparityGuidedFilterImpl::create());
+}
+    
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+
+class DisparityWLSFilterImpl : public DisparityWLSFilter
+{
+protected:
+    double lambda,sigma_color;
+    void init(double _lambda, double _sigma_color)
+    {
+        lambda = _lambda;
+        sigma_color = _sigma_color;
+    }
+public:
+    double getLambda() {return lambda;}
+    void setLambda(double _lambda) {lambda = _lambda;}
+    double getSigmaColor() {return sigma_color;}
+    void setSigmaColor(double _sigma_color) {sigma_color = _sigma_color;}
+
+    static Ptr<DisparityWLSFilterImpl> create()
+    {
+        DisparityWLSFilterImpl *wls = new DisparityWLSFilterImpl();
+        wls->init(500.0,2.0);
+        return Ptr<DisparityWLSFilterImpl>(wls);
+    }
+
+    void filter(InputArray disparity_map, InputArray left_view, OutputArray filtered_disparity_map,Rect ROI)
+    {
+        Mat disp,src,dst;
+        setROI(disparity_map,left_view,filtered_disparity_map,disp,src,dst,ROI);
+        weightedLeastSquaresFilter(src,disp,dst,lambda,sigma_color);
+    }
+};
+
+CV_EXPORTS_W
+Ptr<DisparityWLSFilter> createDisparityWLSFilter()
+{
+    return Ptr<DisparityWLSFilter>(DisparityWLSFilterImpl::create());
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void setROI(InputArray disparity_map, InputArray left_view, OutputArray filtered_disparity_map,
+    Mat& disp,Mat& src,Mat& dst,Rect ROI)
+{
+    Mat disp_full_size = disparity_map.getMat();
+    Mat src_full_size = left_view.getMat();
+    CV_Assert( (disp_full_size.depth() == CV_16S) && (disp_full_size.channels() == 1) );
+    CV_Assert( (src_full_size.depth()  == CV_8U)  && (src_full_size.channels()  <= 4) );
+    disp = Mat(disp_full_size,ROI);
+    src  = Mat(src_full_size ,ROI);
+    filtered_disparity_map.create(disp_full_size.size(), disp_full_size.type());
+    Mat& dst_full_size = filtered_disparity_map.getMatRef();
+    dst = Mat(dst_full_size,ROI);
+}
+
+}
+}

modules/ximgproc/src/wls_filter.cpp

@@ -0,0 +1,284 @@
+/*
+ *  By downloading, copying, installing or using the software you agree to this license.
+ *  If you do not agree to this license, do not download, install,
+ *  copy or use the software.
+ *  
+ *  
+ *  License Agreement
+ *  For Open Source Computer Vision Library
+ *  (3 - clause BSD License)
+ *  
+ *  Redistribution and use in source and binary forms, with or without modification,
+ *  are permitted provided that the following conditions are met :
+ *  
+ *  *Redistributions of source code must retain the above copyright notice,
+ *  this list of conditions and the following disclaimer.
+ *  
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *  this list of conditions and the following disclaimer in the documentation
+ *  and / or other materials provided with the distribution.
+ *  
+ *  * Neither the names of the copyright holders nor the names of the contributors
+ *  may be used to endorse or promote products derived from this software
+ *  without specific prior written permission.
+ *  
+ *  This software is provided by the copyright holders and contributors "as is" and
+ *  any express or implied warranties, including, but not limited to, the implied
+ *  warranties of merchantability and fitness for a particular purpose are disclaimed.
+ *  In no event shall copyright holders or contributors be liable for any direct,
+ *  indirect, incidental, special, exemplary, or consequential damages
+ *  (including, but not limited to, procurement of substitute goods or services;
+ *  loss of use, data, or profits; or business interruption) however caused
+ *  and on any theory of liability, whether in contract, strict liability,
+ *  or tort(including negligence or otherwise) arising in any way out of
+ *  the use of this software, even if advised of the possibility of such damage.
+ */
+
+#include "precomp.hpp"
+
+namespace cv {
+namespace ximgproc {
+
+class WeightedLeastSquaresFilterImpl : public WeightedLeastSquaresFilter
+{
+public:
+    static Ptr<WeightedLeastSquaresFilterImpl> create(InputArray guide, double lambda, double sigma_color, int num_iter);
+    void filter(InputArray src, OutputArray dst);
+    ~WeightedLeastSquaresFilterImpl();
+protected:
+    int w,h;
+    double sigmaColor,lambda;
+    int num_iter;
+    double *weights_LUT;
+    double *Ahor,  *Bhor,  *Chor, 
+            *Avert, *Bvert, *Cvert;
+    double *interD,*interE,*cur_res;
+    void init(InputArray guide,double _lambda,double _sigmaColor,int _num_iter);
+    void buildCoefMatrices(Mat& guide);
+    void horizontalPass(double* cur);
+    void verticalPass(double* cur);
+};
+
+void WeightedLeastSquaresFilterImpl::init(InputArray guide,double _lambda,double _sigmaColor,int _num_iter)
+{
+    //currently support only 3 channel 8bit images as guides
+    CV_Assert( !guide.empty() && _lambda >= 0 && _sigmaColor >= 0 && _num_iter >=1 );
+    CV_Assert( guide.depth() == CV_8U && guide.channels() == 3 );
+    sigmaColor = _sigmaColor;
+    lambda = _lambda;
+    num_iter = _num_iter;
+    int num_levels = 3*256*256;
+    weights_LUT = new double[num_levels];
+    for(int i=0;i<num_levels;i++)
+        weights_LUT[i] = exp(-sqrt((double)i)/sigmaColor);
+    w = guide.cols();
+    h = guide.rows();
+    int sz = w*h;
+    Ahor  = new double[sz];Bhor  = new double[sz];Chor  = new double[sz];
+    Avert = new double[sz];Bvert = new double[sz];Cvert = new double[sz];
+    interD = new double[sz];interE = new double[sz];cur_res = new double[sz];
+    Mat guideMat = guide.getMat();
+    buildCoefMatrices(guideMat);
+}
+
+Ptr<WeightedLeastSquaresFilterImpl> WeightedLeastSquaresFilterImpl::create(InputArray guide, double lambda, double sigma_color, int num_iter)
+{
+    WeightedLeastSquaresFilterImpl *wls = new WeightedLeastSquaresFilterImpl();
+    wls->init(guide,lambda,sigma_color,num_iter);
+    return Ptr<WeightedLeastSquaresFilterImpl>(wls);
+}
+
+WeightedLeastSquaresFilter::~WeightedLeastSquaresFilter(){}
+WeightedLeastSquaresFilterImpl::~WeightedLeastSquaresFilterImpl()
+{
+    delete[] weights_LUT;
+    delete[] Ahor;  delete[] Bhor;  delete[] Chor;
+    delete[] Avert; delete[] Bvert; delete[] Cvert;
+    delete[] interD;delete[] interE;delete[] cur_res;
+}
+
+void WeightedLeastSquaresFilterImpl::buildCoefMatrices(Mat& guide)
+{
+    double hor_weight;
+    const unsigned char *row,*row_prev,*row_next;
+    for(int i=0;i<h;i++)
+    {
+        //compute horizontal coefs:
+        row = guide.ptr(i);
+        Ahor[i*w] = 0;
+        hor_weight = weights_LUT[ (row[0]-row[3])*(row[0]-row[3])+
+                                  (row[1]-row[4])*(row[1]-row[4])+
+                                  (row[2]-row[5])*(row[2]-row[5]) ];
+        Chor[i*w] = -lambda*hor_weight;
+        Bhor[i*w] = 1 - Ahor[i*w] - Chor[i*w];
+        row+=3;
+        for(int j=1;j<w-1;j++)
+        {
+            Ahor[i*w+j] = -lambda*hor_weight;
+            hor_weight = weights_LUT[ (row[0]-row[3])*(row[0]-row[3])+
+                                      (row[1]-row[4])*(row[1]-row[4])+
+                                      (row[2]-row[5])*(row[2]-row[5]) ];
+            Chor[i*w+j] = -lambda*hor_weight;
+            Bhor[i*w+j] = 1 - Ahor[i*w+j] - Chor[i*w+j];
+            row+=3;
+        }
+        Ahor[i*w+w-1] = -lambda*hor_weight;
+        Chor[i*w+w-1] = 0;
+        Bhor[i*w+w-1] = 1 - Ahor[i*w+w-1] - Chor[i*w+w-1];
+
+        //compute vertical coefs:
+        row = guide.ptr(i);
+        if(i==0)
+        {
+            row_next = guide.ptr(i+1);
+            for(int j=0;j<w;j++)
+            {
+                Avert[i*w+j] = 0;
+                Cvert[i*w+j] = -lambda*weights_LUT[ (row[0]-row_next[0])*(row[0]-row_next[0])+
+                                                    (row[1]-row_next[1])*(row[1]-row_next[1])+
+                                                    (row[2]-row_next[2])*(row[2]-row_next[2]) ];
+                Bvert[i*w+j] = 1 - Avert[i*w+j] - Cvert[i*w+j];
+                row+=3;
+                row_next+=3;
+            }
+        }
+        else if(i==h-1)
+        {
+            row_prev = guide.ptr(i-1);
+            for(int j=0;j<w;j++)
+            {
+                Avert[i*w+j] = -lambda*weights_LUT[ (row[0]-row_prev[0])*(row[0]-row_prev[0])+
+                                                    (row[1]-row_prev[1])*(row[1]-row_prev[1])+
+                                                    (row[2]-row_prev[2])*(row[2]-row_prev[2]) ];
+                Cvert[i*w+j] = 0;
+                Bvert[i*w+j] = 1 - Avert[i*w+j] - Cvert[i*w+j];
+                row+=3;
+                row_prev+=3;
+            }
+        }
+        else
+        {
+            row_prev = guide.ptr(i-1);
+            row_next = guide.ptr(i+1);
+            for(int j=0;j<w;j++)
+            {
+                Avert[i*w+j] = -lambda*weights_LUT[ (row[0]-row_prev[0])*(row[0]-row_prev[0])+
+                                                    (row[1]-row_prev[1])*(row[1]-row_prev[1])+
+                                                    (row[2]-row_prev[2])*(row[2]-row_prev[2]) ];
+                Cvert[i*w+j] = -lambda*weights_LUT[ (row[0]-row_next[0])*(row[0]-row_next[0])+
+                    (row[1]-row_next[1])*(row[1]-row_next[1])+
+                    (row[2]-row_next[2])*(row[2]-row_next[2]) ];
+                Bvert[i*w+j] = 1 - Avert[i*w+j] - Cvert[i*w+j];
+                row+=3;
+                row_prev+=3;
+                row_next+=3;
+            }
+        }
+    }
+}
+
+void WeightedLeastSquaresFilterImpl::filter(InputArray src, OutputArray dst)
+{
+    //temporarily support only one-channel CV_16S src type (for disparity map filtering)
+    CV_Assert(!src.empty() && (src.depth() == CV_16S) && src.channels()==1);
+    if (src.rows() != h || src.cols() != w)
+    {
+        CV_Error(Error::StsBadSize, "Size of filtering image must be equal to size of guide image");
+        return;
+    }
+
+    Mat srcMat = src.getMat(); 
+    Mat& dstMat = dst.getMatRef();
+    short* row;
+    for(int i=0;i<h;i++)
+    {
+        row = (short*)srcMat.ptr(i);
+        for(int j=0;j<w;j++)
+        {
+            cur_res[i*w+j] = (double)(*row);
+            row++;
+        }
+    }
+
+    for(int n=0;n<num_iter;n++)
+    {
+        horizontalPass(cur_res);
+        verticalPass(cur_res);
+    }
+
+    for(int i=0;i<h;i++)
+    {
+        row = (short*)dstMat.ptr(i);
+        for(int j=0;j<w;j++)
+        {
+            *row = saturate_cast<short>(cur_res[i*w+j]);
+            row++;
+        }
+    }
+}
+
+void WeightedLeastSquaresFilterImpl::horizontalPass(double* cur)
+{
+    double denom;
+    for(int i=0;i<h;i++)
+    {
+        //forward pass:
+        interD[i*w] = Chor[i*w]/Bhor[i*w];
+        interE[i*w] = cur[i*w] /Bhor[i*w];
+        for(int j=1;j<w;j++)
+        {
+            denom = Bhor[i*w+j]-interD[i*w+j-1]*Ahor[i*w+j];
+            interD[i*w+j] = Chor[i*w+j]/denom;
+            interE[i*w+j] = (cur[i*w+j]-interE[i*w+j-1]*Ahor[i*w+j])/denom;
+        }
+
+        //backward pass:
+        cur[i*w+w-1] = interE[i*w+w-1];
+        for(int j=w-2;j>=0;j--)
+            cur[i*w+j] = interE[i*w+j]-interD[i*w+j]*cur[i*w+j+1];
+    }
+}
+
+void WeightedLeastSquaresFilterImpl::verticalPass(double* cur)
+{
+    double denom;
+    //forward pass:
+    for(int j=0;j<w;j++)
+    {
+        interD[j] = Cvert[j]/Bvert[j];
+        interE[j] = cur[j]/Bvert[j];
+    }
+    for(int i=1;i<h;i++)
+    {
+        for(int j=0;j<w;j++)
+        {
+            denom = Bvert[i*w+j]-interD[(i-1)*w+j]*Avert[i*w+j];
+            interD[i*w+j] = Cvert[i*w+j]/denom;
+            interE[i*w+j] = (cur[i*w+j]-interE[(i-1)*w+j]*Avert[i*w+j])/denom;
+        }
+    }
+    //backward pass:
+    for(int j=0;j<w;j++)
+        cur[(h-1)*w+j] = interE[(h-1)*w+j];
+    for(int i=h-2;i>=0;i--)
+        for(int j=w-1;j>=0;j--)
+            cur[i*w+j] = interE[i*w+j]-interD[i*w+j]*cur[(i+1)*w+j];
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+CV_EXPORTS_W
+Ptr<WeightedLeastSquaresFilter> createWeightedLeastSquaresFilter(InputArray guide, double lambda, double sigma_color, int num_iter)
+{
+    return Ptr<WeightedLeastSquaresFilter>(WeightedLeastSquaresFilterImpl::create(guide, lambda, sigma_color, num_iter));
+}
+
+CV_EXPORTS_W
+void weightedLeastSquaresFilter(InputArray guide, InputArray src, OutputArray dst, double lambda, double sigma_color, int num_iter)
+{
+    Ptr<WeightedLeastSquaresFilter> wls = createWeightedLeastSquaresFilter(guide, lambda, sigma_color, num_iter);
+    wls->filter(src, dst);
+}
+
+}
+}