Merge pull request #1883 from ytyytyyt:3.4

* add fast bilateral solver from master

* fix unused variables warnings in disparity_filtering.cpp

* trailing whitespaces removed

modules/ximgproc/doc/ximgproc.bib

@@ -301,3 +301,12 @@
   url = {http://reference.wolfram.com/language/ref/RidgeFilter.html}
 
 }
+
+@inproceedings{BarronPoole2016,
+author = {Jonathan T Barron and Ben Poole},
+title={The Fast Bilateral Solver},
+booktitle={European Conference on Computer Vision (ECCV)},
+year={2016},
+publisher={Springer International Publishing},
+pages={617--632},
+}

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

@@ -375,6 +375,71 @@ void rollingGuidanceFilter(InputArray src, OutputArray dst, int d = -1, double s
 //////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////
 
+/** @brief Interface for implementations of The Fast Bilateral Solver.
+
+For more details about this solver see @cite BarronPoole2016 .
+*/
+class CV_EXPORTS_W FastBilateralSolverFilter : public Algorithm
+{
+public:
+    /** @brief Apply smoothing operation to the source image.
+
+    @param src source image for filtering with unsigned 8-bit or signed 16-bit or floating-point 32-bit depth and up to 3 channels.
+
+    @param confidence confidence image with unsigned 8-bit or floating-point 32-bit confidence and 1 channel.
+
+    @param dst destination image.
+    */
+    CV_WRAP virtual void filter(InputArray src, InputArray confidence, OutputArray dst) = 0;
+};
+
+/** @brief Factory method, create instance of FastBilateralSolverFilter and execute the initialization routines.
+
+@param guide image serving as guide for filtering. It should have 8-bit depth and either 1 or 3 channels.
+
+@param sigma_spatial parameter, that is similar to spatial space sigma in bilateralFilter.
+
+@param sigma_luma parameter, that is similar to luma space sigma in bilateralFilter.
+
+@param sigma_chroma parameter, that is similar to chroma space sigma in bilateralFilter.
+
+@param num_iter number of iterations used for solving, 25 is usually enough.
+
+@param max_tol solving tolerance used for solving.
+
+For more details about the Fast Bilateral Solver parameters, see the original paper @cite BarronPoole2016.
+
+*/
+CV_EXPORTS_W Ptr<FastBilateralSolverFilter> createFastBilateralSolverFilter(InputArray guide, double sigma_spatial, double sigma_luma, double sigma_chroma, int num_iter = 25, double max_tol = 1e-5);
+
+/** @brief Simple one-line Fast Bilateral Solver filter call. If you have multiple images to filter with the same
+guide then use FastBilateralSolverFilter interface to avoid extra computations.
+
+@param guide image serving as guide for filtering. It should have 8-bit depth and either 1 or 3 channels.
+
+@param src source image for filtering with unsigned 8-bit or signed 16-bit or floating-point 32-bit depth and up to 4 channels.
+
+@param confidence confidence image with unsigned 8-bit or floating-point 32-bit confidence and 1 channel.
+
+@param dst destination image.
+
+@param sigma_spatial parameter, that is similar to spatial space sigma in bilateralFilter.
+
+@param sigma_luma parameter, that is similar to luma space sigma in bilateralFilter.
+
+@param sigma_chroma parameter, that is similar to chroma space sigma in bilateralFilter.
+
+@param num_iter number of iterations used for solving, 25 is usually enough.
+
+@param max_tol solving tolerance used for solving.
+
+@note Confidence images with CV_8U depth are expected to in [0, 255] and CV_32F in [0, 1] range.
+*/
+CV_EXPORTS_W void fastBilateralSolverFilter(InputArray guide, InputArray src, InputArray confidence, OutputArray dst, double sigma_spatial = 8, double sigma_luma = 8, double sigma_chroma = 8, int num_iter = 25, double max_tol = 1e-5);
+
+//////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////
+
 
 /** @brief Interface for implementations of Fast Global Smoother filter.
 

modules/ximgproc/samples/colorize.cpp

+
+#include "opencv2/core.hpp"
+#include "opencv2/highgui.hpp"
+#include "opencv2/imgproc.hpp"
+#include "opencv2/core/utility.hpp"
+
+#include <time.h>
+#include <vector>
+#include <iostream>
+#include <opencv2/ximgproc.hpp>
+
+
+
+using namespace cv;
+
+#ifdef HAVE_EIGEN
+
+#define MARK_RADIUS 5
+#define PALLET_RADIUS 100
+int max_width = 1280;
+int max_height = 720;
+
+static int globalMouseX;
+static int globalMouseY;
+static int selected_r;
+static int selected_g;
+static int selected_b;
+static bool globalMouseClick = false;
+static bool glb_mouse_left = false;
+static bool drawByReference = false;
+static bool mouseDraw = false;
+static bool mouseClick;
+static bool mouseLeft;
+static int mouseX;
+static int mouseY;
+
+cv::Mat mat_draw;
+cv::Mat mat_input_gray;
+cv::Mat mat_input_reference;
+cv::Mat mat_input_confidence;
+cv::Mat mat_pallet(PALLET_RADIUS*2,PALLET_RADIUS*2,CV_8UC3);
+
+
+static void mouseCallback(int event, int x, int y, int flags, void* param);
+void drawTrajectoryByReference(cv::Mat& img);
+double module(Point pt);
+double distance(Point pt1, Point pt2);
+double cross(Point pt1, Point pt2);
+double angle(Point pt1, Point pt2);
+int inCircle(Point p, Point c, int r);
+void createPlate(Mat &im1, int radius);
+
+
+#endif
+
+const String keys =
+    "{help h usage ?     |                | print this message                                                }"
+    "{@image             |                | input image                                                       }"
+    "{sigma_spatial      |8               | parameter of post-filtering                                       }"
+    "{sigma_luma         |8               | parameter of post-filtering                                       }"
+    "{sigma_chroma       |8               | parameter of post-filtering                                       }"
+    "{dst_path           |None            | optional path to save the resulting colorized image               }"
+    "{dst_raw_path       |None            | optional path to save drawed image before filtering               }"
+    "{draw_by_reference  |false           | optional flag to use color image as reference                     }"
+    ;
+
+
+
+int main(int argc, char* argv[])
+{
+
+    CommandLineParser parser(argc,argv,keys);
+    parser.about("fastBilateralSolverFilter Demo");
+    if (parser.has("help"))
+    {
+        parser.printMessage();
+        return 0;
+    }
+
+#ifdef HAVE_EIGEN
+
+    String img = parser.get<String>(0);
+    double sigma_spatial  = parser.get<double>("sigma_spatial");
+    double sigma_luma  = parser.get<double>("sigma_luma");
+    double sigma_chroma  = parser.get<double>("sigma_chroma");
+    String dst_path = parser.get<String>("dst_path");
+    String dst_raw_path = parser.get<String>("dst_raw_path");
+    drawByReference = parser.get<bool>("draw_by_reference");
+
+    mat_input_reference = cv::imread(img, IMREAD_COLOR);
+    if (mat_input_reference.empty())
+    {
+        std::cerr << "input image '" << img << "' could not be read !" << std::endl << std::endl;
+        parser.printMessage();
+        return 1;
+    }
+
+    cvtColor(mat_input_reference, mat_input_gray, COLOR_BGR2GRAY);
+
+    if(mat_input_gray.cols > max_width)
+    {
+        double scale = float(max_width) / float(mat_input_gray.cols);
+        cv::resize(mat_input_reference, mat_input_reference, cv::Size(), scale, scale);
+        cv::resize(mat_input_gray, mat_input_gray, cv::Size(), scale, scale);
+    }
+
+    if(mat_input_gray.rows > max_height)
+    {
+        double scale = float(max_height) / float(mat_input_gray.rows);
+        cv::resize(mat_input_reference, mat_input_reference, cv::Size(), scale, scale);
+        cv::resize(mat_input_gray, mat_input_gray, cv::Size(), scale, scale);
+    }
+
+
+    float filtering_time;
+    std::cout << "mat_input_reference:" << mat_input_reference.cols<<"x"<< mat_input_reference.rows<< std::endl;
+    std::cout << "please select a color from the palette, by clicking into that," << std::endl;
+    std::cout << "  then select a coarse region in the image to be coloured." << std::endl;
+    std::cout << "  press 'escape' to see the final coloured image." << std::endl;
+
+
+    cv::Mat mat_gray;
+    cv::cvtColor(mat_input_reference, mat_gray, cv::COLOR_BGR2GRAY);
+
+    cv::Mat target = mat_input_reference.clone();
+    cvtColor(mat_gray, mat_input_reference, COLOR_GRAY2BGR);
+
+    cv::namedWindow("draw", cv::WINDOW_AUTOSIZE);
+
+    // construct pallet
+    createPlate(mat_pallet, PALLET_RADIUS);
+    selected_b = 0;
+    selected_g = 0;
+    selected_r = 0;
+
+    cv::Mat mat_show(target.rows,target.cols+PALLET_RADIUS*2,CV_8UC3);
+    cv::Mat color_select(target.rows-mat_pallet.rows,PALLET_RADIUS*2,CV_8UC3,cv::Scalar(selected_b, selected_g, selected_r));
+    target.copyTo(Mat(mat_show,Rect(0,0,target.cols,target.rows)));
+    mat_pallet.copyTo(Mat(mat_show,Rect(target.cols,0,mat_pallet.cols,mat_pallet.rows)));
+    color_select.copyTo(Mat(mat_show,Rect(target.cols,PALLET_RADIUS*2,color_select.cols,color_select.rows)));
+
+    cv::imshow("draw", mat_show);
+    cv::setMouseCallback("draw", mouseCallback, (void *)&mat_show);
+    mat_input_confidence = 0*cv::Mat::ones(mat_gray.size(),mat_gray.type());
+
+    int show_count = 0;
+    while (1)
+    {
+            mouseX = globalMouseX;
+            mouseY = globalMouseY;
+            mouseClick = globalMouseClick;
+            mouseLeft = glb_mouse_left;
+
+
+        if (mouseClick)
+        {
+            drawTrajectoryByReference(target);
+
+            if(show_count%5==0)
+            {
+                cv::Mat target_temp(target.size(),target.type());
+                filtering_time = (double)getTickCount();
+                if(mouseDraw)
+                {
+                    cv::cvtColor(target, target_temp, cv::COLOR_BGR2YCrCb);
+                    std::vector<cv::Mat> src_channels;
+                    std::vector<cv::Mat> dst_channels;
+
+                    cv::split(target_temp,src_channels);
+
+                    cv::Mat result1 = cv::Mat(mat_input_gray.size(),mat_input_gray.type());
+                    cv::Mat result2 = cv::Mat(mat_input_gray.size(),mat_input_gray.type());
+
+                    dst_channels.push_back(mat_input_gray);
+                    cv::ximgproc::fastBilateralSolverFilter(mat_input_gray,src_channels[1],mat_input_confidence,result1,sigma_spatial,sigma_luma,sigma_chroma);
+                    dst_channels.push_back(result1);
+                    cv::ximgproc::fastBilateralSolverFilter(mat_input_gray,src_channels[2],mat_input_confidence,result2,sigma_spatial,sigma_luma,sigma_chroma);
+                    dst_channels.push_back(result2);
+
+                    cv::merge(dst_channels,target_temp);
+                    cv::cvtColor(target_temp, target_temp, cv::COLOR_YCrCb2BGR);
+                }
+                else
+                {
+                  target_temp = target.clone();
+                }
+                filtering_time = ((double)getTickCount() - filtering_time)/getTickFrequency();
+                std::cout << "solver time: " << filtering_time << "s" << std::endl;
+
+                cv::Mat color_selected(target_temp.rows-mat_pallet.rows,PALLET_RADIUS*2,CV_8UC3,cv::Scalar(selected_b, selected_g, selected_r));
+                target_temp.copyTo(Mat(mat_show,Rect(0,0,target_temp.cols,target_temp.rows)));
+                mat_pallet.copyTo(Mat(mat_show,Rect(target_temp.cols,0,mat_pallet.cols,mat_pallet.rows)));
+                color_selected.copyTo(Mat(mat_show,Rect(target_temp.cols,PALLET_RADIUS*2,color_selected.cols,color_selected.rows)));
+                cv::imshow("draw", mat_show);
+            }
+            show_count++;
+        }
+        if (cv::waitKey(2) == 27)
+            break;
+    }
+    mat_draw = target.clone();
+    cv::cvtColor(target, target, cv::COLOR_BGR2YCrCb);
+
+    std::vector<cv::Mat> src_channels;
+    std::vector<cv::Mat> dst_channels;
+
+    cv::split(target,src_channels);
+
+    cv::Mat result1 = cv::Mat(mat_input_gray.size(),mat_input_gray.type());
+    cv::Mat result2 = cv::Mat(mat_input_gray.size(),mat_input_gray.type());
+
+    filtering_time = (double)getTickCount();
+
+    // dst_channels.push_back(src_channels[0]);
+    dst_channels.push_back(mat_input_gray);
+    cv::ximgproc::fastBilateralSolverFilter(mat_input_gray,src_channels[1],mat_input_confidence,result1,sigma_spatial,sigma_luma,sigma_chroma);
+    dst_channels.push_back(result1);
+    cv::ximgproc::fastBilateralSolverFilter(mat_input_gray,src_channels[2],mat_input_confidence,result2,sigma_spatial,sigma_luma,sigma_chroma);
+    dst_channels.push_back(result2);
+
+    cv::merge(dst_channels,target);
+    cv::cvtColor(target, target, cv::COLOR_YCrCb2BGR);
+
+    filtering_time = ((double)getTickCount() - filtering_time)/getTickFrequency();
+    std::cout << "solver time: " << filtering_time << "s" << std::endl;
+
+
+
+    cv::imshow("mat_draw",mat_draw);
+    cv::imshow("output",target);
+
+    if(dst_path!="None")
+    {
+        imwrite(dst_path,target);
+    }
+    if(dst_raw_path!="None")
+    {
+        imwrite(dst_raw_path,mat_draw);
+    }
+
+    cv::waitKey(0);
+
+
+
+#else
+    std::cout << "Can not find eigen, please build with eigen by set WITH_EIGEN=ON" << '\n';
+#endif
+
+    return 0;
+}
+
+
+#ifdef HAVE_EIGEN
+static void mouseCallback(int event, int x, int y, int, void*)
+{
+    switch (event)
+    {
+        case cv::EVENT_MOUSEMOVE:
+            if (globalMouseClick)
+            {
+                globalMouseX = x;
+                globalMouseY = y;
+            }
+            break;
+
+        case cv::EVENT_LBUTTONDOWN:
+            globalMouseClick = true;
+            globalMouseX = x;
+            globalMouseY = y;
+            break;
+
+        case cv::EVENT_LBUTTONUP:
+            glb_mouse_left = true;
+            globalMouseClick = false;
+            break;
+    }
+}
+
+void drawTrajectoryByReference(cv::Mat& img)
+{
+    int i, j;
+    uchar red, green, blue;
+    float gray;
+    int y, x;
+    int r = MARK_RADIUS;
+    int r2 = r * r;
+    uchar* colorPix;
+    uchar* grayPix;
+
+    if(mouseY < PALLET_RADIUS*2 && img.cols <= mouseX && mouseX < img.cols+PALLET_RADIUS*2)
+    {
+        colorPix = mat_pallet.ptr<uchar>(mouseY, mouseX - img.cols);
+        // colorPix = mat_pallet.ptr<uchar>(mouseY, mouseX);
+        selected_b = *colorPix;
+        colorPix++;
+        selected_g = *colorPix;
+        colorPix++;
+        selected_r = *colorPix;
+        colorPix++;
+        std::cout << "x y:("<<mouseX<<"," <<mouseY<< " rgb_select:("<< selected_r<<","<<selected_g<<","<<selected_b<<")" << '\n';
+    }
+    else
+    {
+        mouseDraw = true;
+        y = mouseY - r;
+        for(i=-r; i<r+1 ; i++, y++)
+        {
+            x = mouseX - r;
+            colorPix = mat_input_reference.ptr<uchar>(y, x);
+            grayPix = mat_input_gray.ptr<uchar>(y, x);
+            for(j=-r; j<r+1; j++, x++)
+            {
+                if(i*i + j*j > r2)
+                {
+                    colorPix += mat_input_reference.channels();
+                    grayPix += mat_input_gray.channels();
+                    continue;
+                }
+
+                if(y<0 || y>=mat_input_reference.rows || x<0 || x>=mat_input_reference.cols)
+                {
+                    break;
+                }
+
+                blue = *colorPix;
+                colorPix++;
+                green = *colorPix;
+                colorPix++;
+                red = *colorPix;
+                colorPix++;
+                gray = *grayPix;
+                grayPix++;
+                mat_input_confidence.at<uchar>(y,x) = 255;
+                float draw_y = 0.229*(float(selected_r)) + 0.587*(float(selected_g)) + 0.114*(float(selected_b));
+                int draw_b = int(float(selected_b)*(gray/draw_y));
+                int draw_g = int(float(selected_g)*(gray/draw_y));
+                int draw_r = int(float(selected_r)*(gray/draw_y));
+
+                if(drawByReference)
+                {
+                    cv::circle(img, cv::Point2d(x, y), 0.1, cv::Scalar(blue, green, red), -1);
+                }
+                else
+                {
+                    cv::circle(img, cv::Point2d(x, y), 0.1, cv::Scalar(draw_b, draw_g, draw_r), -1);
+                }
+            }
+        }
+    }
+}
+
+double module(Point pt)
+{
+	return sqrt((double)pt.x*pt.x + pt.y*pt.y);
+}
+
+double distance(Point pt1, Point pt2)
+{
+	int dx = pt1.x - pt2.x;
+	int dy = pt1.y - pt2.y;
+	return sqrt((double)dx*dx + dy*dy);
+}
+
+double cross(Point pt1, Point pt2)
+{
+	return pt1.x*pt2.x + pt1.y*pt2.y;
+}
+
+double angle(Point pt1, Point pt2)
+{
+	return acos(cross(pt1, pt2) / (module(pt1)*module(pt2) + DBL_EPSILON));
+}
+
+// p or c is the center
+int inCircle(Point p, Point c, int r)
+{
+	int dx = p.x - c.x;
+	int dy = p.y - c.y;
+	return dx*dx + dy*dy <= r*r ? 1 : 0;
+
+}
+
+//draw the hsv-plate
+void createPlate(Mat &im1, int radius)
+{
+	Mat hsvImag(Size(radius << 1, radius << 1), CV_8UC3, Scalar(0, 0, 255));
+	int w = hsvImag.cols;
+	int h = hsvImag.rows;
+	int cx = w >> 1;
+	int cy = h >> 1;
+	Point pt1(cx, 0);
+
+	for (int j = 0; j < w; j++)
+	{
+		for (int i = 0; i < h; i++)
+		{
+			Point pt2(j - cx, i - cy);
+			if (inCircle(Point(0, 0), pt2, radius))
+			{
+				int theta = angle(pt1, pt2) * 180 / CV_PI;
+				if (i > cx)
+				{
+					theta = -theta + 360;
+				}
+				hsvImag.at<Vec3b>(i, j)[0] = theta / 2;
+				hsvImag.at<Vec3b>(i, j)[1] = module(pt2) / cx * 255;
+				hsvImag.at<Vec3b>(i, j)[2] = 255;
+			}
+		}
+	}
+
+
+	cvtColor(hsvImag, im1, COLOR_HSV2BGR);
+}
+
+
+#endif

modules/ximgproc/samples/disparity_filtering.cpp

@@ -4,6 +4,7 @@
 #include "opencv2/highgui.hpp"
 #include "opencv2/core/utility.hpp"
 #include "opencv2/ximgproc/disparity_filter.hpp"
+#include "opencv2/ximgproc.hpp"
 #include <iostream>
 #include <string>
 
@@ -21,15 +22,18 @@ const String keys =
     "{dst_path       |None              | optional path to save the resulting filtered disparity map        }"
     "{dst_raw_path   |None              | optional path to save raw disparity map before filtering          }"
     "{algorithm      |bm                | stereo matching method (bm or sgbm)                               }"
-    "{filter         |wls_conf          | used post-filtering (wls_conf or wls_no_conf)                     }"
+    "{filter         |wls_conf          | used post-filtering (wls_conf or wls_no_conf or fbs_conf)         }"
     "{no-display     |                  | don't display results                                             }"
     "{no-downscale   |                  | force stereo matching on full-sized views to improve quality      }"
     "{dst_conf_path  |None              | optional path to save the confidence map used in filtering        }"
     "{vis_mult       |1.0               | coefficient used to scale disparity map visualizations            }"
     "{max_disparity  |160               | parameter of stereo matching                                      }"
     "{window_size    |-1                | parameter of stereo matching                                      }"
-    "{wls_lambda     |8000.0            | parameter of post-filtering                                       }"
-    "{wls_sigma      |1.5               | parameter of post-filtering                                       }"
+    "{wls_lambda     |8000.0            | parameter of wls post-filtering                                   }"
+    "{wls_sigma      |1.5               | parameter of wls post-filtering                                   }"
+    "{fbs_spatial    |16.0              | parameter of fbs post-filtering                                   }"
+    "{fbs_luma       |8.0               | parameter of fbs post-filtering                                   }"
+    "{fbs_chroma     |8.0               | parameter of fbs post-filtering                                   }"
     ;
 
 int main(int argc, char** argv)
@@ -56,6 +60,9 @@ int main(int argc, char** argv)
     int max_disp = parser.get<int>("max_disparity");
     double lambda = parser.get<double>("wls_lambda");
     double sigma  = parser.get<double>("wls_sigma");
+    double fbs_spatial = parser.get<double>("fbs_spatial");
+    double fbs_luma = parser.get<double>("fbs_luma");
+    double fbs_chroma = parser.get<double>("fbs_chroma");
     double vis_mult = parser.get<double>("vis_mult");
 
     int wsize;
@@ -109,12 +116,13 @@ int main(int argc, char** argv)
 
     Mat left_for_matcher, right_for_matcher;
     Mat left_disp,right_disp;
-    Mat filtered_disp;
+    Mat filtered_disp,solved_disp,solved_filtered_disp;
     Mat conf_map = Mat(left.rows,left.cols,CV_8U);
     conf_map = Scalar(255);
     Rect ROI;
     Ptr<DisparityWLSFilter> wls_filter;
     double matching_time, filtering_time;
+    double solving_time = 0;
     if(max_disp<=0 || max_disp%16!=0)
     {
         cout<<"Incorrect max_disparity value: it should be positive and divisible by 16";
@@ -125,6 +133,7 @@ int main(int argc, char** argv)
         cout<<"Incorrect window_size value: it should be positive and odd";
         return -1;
     }
+
     if(filter=="wls_conf") // filtering with confidence (significantly better quality than wls_no_conf)
     {
         if(!no_downscale)
@@ -201,6 +210,104 @@ int main(int argc, char** argv)
             ROI = Rect(ROI.x*2,ROI.y*2,ROI.width*2,ROI.height*2);
         }
     }
+    else if(filter=="fbs_conf") // filtering with fbs and confidence using also wls pre-processing
+    {
+        if(!no_downscale)
+        {
+            // downscale the views to speed-up the matching stage, as we will need to compute both left
+            // and right disparity maps for confidence map computation
+            //! [downscale_wls]
+            max_disp/=2;
+            if(max_disp%16!=0)
+                max_disp += 16-(max_disp%16);
+            resize(left ,left_for_matcher ,Size(),0.5,0.5);
+            resize(right,right_for_matcher,Size(),0.5,0.5);
+            //! [downscale_wls]
+        }
+        else
+        {
+            left_for_matcher  = left.clone();
+            right_for_matcher = right.clone();
+        }
+
+        if(algo=="bm")
+        {
+            //! [matching_wls]
+            Ptr<StereoBM> left_matcher = StereoBM::create(max_disp,wsize);
+            wls_filter = createDisparityWLSFilter(left_matcher);
+            Ptr<StereoMatcher> right_matcher = createRightMatcher(left_matcher);
+
+            cvtColor(left_for_matcher,  left_for_matcher,  COLOR_BGR2GRAY);
+            cvtColor(right_for_matcher, right_for_matcher, COLOR_BGR2GRAY);
+
+            matching_time = (double)getTickCount();
+            left_matcher-> compute(left_for_matcher, right_for_matcher,left_disp);
+            right_matcher->compute(right_for_matcher,left_for_matcher, right_disp);
+            matching_time = ((double)getTickCount() - matching_time)/getTickFrequency();
+            //! [matching_wls]
+        }
+        else if(algo=="sgbm")
+        {
+            Ptr<StereoSGBM> left_matcher  = StereoSGBM::create(0,max_disp,wsize);
+            left_matcher->setP1(24*wsize*wsize);
+            left_matcher->setP2(96*wsize*wsize);
+            left_matcher->setPreFilterCap(63);
+            left_matcher->setMode(StereoSGBM::MODE_SGBM_3WAY);
+            wls_filter = createDisparityWLSFilter(left_matcher);
+            Ptr<StereoMatcher> right_matcher = createRightMatcher(left_matcher);
+
+            matching_time = (double)getTickCount();
+            left_matcher-> compute(left_for_matcher, right_for_matcher,left_disp);
+            right_matcher->compute(right_for_matcher,left_for_matcher, right_disp);
+            matching_time = ((double)getTickCount() - matching_time)/getTickFrequency();
+        }
+        else
+        {
+            cout<<"Unsupported algorithm";
+            return -1;
+        }
+
+        //! [filtering_wls]
+        wls_filter->setLambda(lambda);
+        wls_filter->setSigmaColor(sigma);
+        filtering_time = (double)getTickCount();
+        wls_filter->filter(left_disp,left,filtered_disp,right_disp);
+        filtering_time = ((double)getTickCount() - filtering_time)/getTickFrequency();
+        //! [filtering_wls]
+
+        conf_map = wls_filter->getConfidenceMap();
+
+        Mat left_disp_resized;
+        resize(left_disp,left_disp_resized,left.size());
+
+        // Get the ROI that was used in the last filter call:
+        ROI = wls_filter->getROI();
+        if(!no_downscale)
+        {
+            // upscale raw disparity and ROI back for a proper comparison:
+            resize(left_disp,left_disp,Size(),2.0,2.0);
+            left_disp = left_disp*2.0;
+            left_disp_resized = left_disp_resized*2.0;
+            ROI = Rect(ROI.x*2,ROI.y*2,ROI.width*2,ROI.height*2);
+        }
+
+#ifdef HAVE_EIGEN
+        //! [filtering_fbs]
+        solving_time = (double)getTickCount();
+        fastBilateralSolverFilter(left, left_disp_resized, conf_map/255.0f, solved_disp, fbs_spatial, fbs_luma, fbs_chroma);
+        solving_time = ((double)getTickCount() - solving_time)/getTickFrequency();
+        //! [filtering_fbs]
+
+        //! [filtering_wls2fbs]
+        fastBilateralSolverFilter(left, filtered_disp, conf_map/255.0f, solved_filtered_disp, fbs_spatial, fbs_luma, fbs_chroma);
+        //! [filtering_wls2fbs]
+#else
+        (void)fbs_spatial;
+        (void)fbs_luma;
+        (void)fbs_chroma;
+
+#endif
+    }
     else if(filter=="wls_no_conf")
     {
         /* There is no convenience function for the case of filtering with no confidence, so we
@@ -263,6 +370,7 @@ int main(int argc, char** argv)
     cout.precision(2);
     cout<<"Matching time:  "<<matching_time<<"s"<<endl;
     cout<<"Filtering time: "<<filtering_time<<"s"<<endl;
+    cout<<"Solving time: "<<solving_time<<"s"<<endl;
     cout<<endl;
 
     double MSE_before,percent_bad_before,MSE_after,percent_bad_after;
@@ -323,7 +431,26 @@ int main(int argc, char** argv)
         getDisparityVis(filtered_disp,filtered_disp_vis,vis_mult);
         namedWindow("filtered disparity", WINDOW_AUTOSIZE);
         imshow("filtered disparity", filtered_disp_vis);
-        waitKey();
+
+        if(!solved_disp.empty())
+        {
+            Mat solved_disp_vis;
+            getDisparityVis(solved_disp,solved_disp_vis,vis_mult);
+            namedWindow("solved disparity", WINDOW_AUTOSIZE);
+            imshow("solved disparity", solved_disp_vis);
+
+            Mat solved_filtered_disp_vis;
+            getDisparityVis(solved_filtered_disp,solved_filtered_disp_vis,vis_mult);
+            namedWindow("solved wls disparity", WINDOW_AUTOSIZE);
+            imshow("solved wls disparity", solved_filtered_disp_vis);
+        }
+
+        while(1)
+        {
+            char key = (char)waitKey();
+            if( key == 27 || key == 'q' || key == 'Q') // 'ESC'
+                break;
+        }
         //! [visualization]
     }
 

modules/ximgproc/src/fbs_filter.cpp

+/*
+ *  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 <cmath>
+#include <vector>
+#include <memory>
+#include <stdlib.h>
+#include <iostream>
+#include <iterator>
+#include <algorithm>
+
+
+#ifdef HAVE_EIGEN
+#  if defined __GNUC__ && defined __APPLE__
+#    pragma GCC diagnostic ignored "-Wshadow"
+#  endif
+#  include <Eigen/Dense>
+#  include <Eigen/SparseCore>
+#  include <Eigen/SparseCholesky>
+#  include <Eigen/IterativeLinearSolvers>
+#  include <Eigen/Sparse>
+
+
+
+
+#if __cplusplus <= 199711L
+    #include <map>
+    typedef std::map<long long /* hash */, int /* vert id */>  mapId;
+#else
+    #include <unordered_map>
+    typedef std::unordered_map<long long /* hash */, int /* vert id */>  mapId;
+#endif
+
+#define EPS 1e-43f
+
+namespace cv
+{
+namespace ximgproc
+{
+
+    class FastBilateralSolverFilterImpl : public FastBilateralSolverFilter
+    {
+    public:
+
+        static Ptr<FastBilateralSolverFilterImpl> create(InputArray guide, double sigma_spatial, double sigma_luma, double sigma_chroma, int num_iter, double max_tol)
+        {
+            CV_Assert(guide.type() == CV_8UC1 || guide.type() == CV_8UC3);
+            FastBilateralSolverFilterImpl *fbs = new FastBilateralSolverFilterImpl();
+            Mat gui = guide.getMat();
+            fbs->init(gui,sigma_spatial,sigma_luma,sigma_chroma,num_iter,max_tol);
+            return Ptr<FastBilateralSolverFilterImpl>(fbs);
+        }
+
+        void filter(InputArray src, InputArray confidence, OutputArray dst) CV_OVERRIDE
+        {
+
+            CV_Assert(!src.empty() && (src.depth() == CV_8U || src.depth() == CV_16S || src.depth() == CV_32F) && src.channels()<=4);
+            CV_Assert(!confidence.empty() && (confidence.depth() == CV_8U || confidence.depth() == CV_32F) && confidence.channels()==1);
+            if (src.rows() != rows || src.cols() != cols)
+            {
+                CV_Error(Error::StsBadSize, "Size of the filtered image must be equal to the size of the guide image");
+                return;
+            }
+            if (confidence.rows() != rows || confidence.cols() != cols)
+            {
+                CV_Error(Error::StsBadSize, "Size of the confidence image must be equal to the size of the guide image");
+                return;
+            }
+
+            std::vector<Mat> src_channels;
+            std::vector<Mat> dst_channels;
+            if(src.channels()==1)
+                src_channels.push_back(src.getMat());
+            else
+                split(src,src_channels);
+
+            Mat conf = confidence.getMat();
+
+            for(int i=0;i<src.channels();i++)
+            {
+                Mat cur_res = src_channels[i].clone();
+
+                solve(cur_res,conf,cur_res);
+                cur_res.convertTo(cur_res, src.type());
+                dst_channels.push_back(cur_res);
+            }
+
+            dst.create(src.size(),src_channels[0].type());
+            if(src.channels()==1)
+            {
+                Mat& dstMat = dst.getMatRef();
+                dstMat = dst_channels[0];
+            }
+            else
+            merge(dst_channels,dst);
+            CV_Assert(src.type() == dst.type() && src.size() == dst.size());
+        }
+
+    // protected:
+        void solve(cv::Mat& src, cv::Mat& confidence, cv::Mat& dst);
+        void init(cv::Mat& reference, double sigma_spatial, double sigma_luma, double sigma_chroma, int num_iter, double max_tol);
+
+        void Splat(Eigen::VectorXf& input, Eigen::VectorXf& dst);
+        void Blur(Eigen::VectorXf& input, Eigen::VectorXf& dst);
+        void Slice(Eigen::VectorXf& input, Eigen::VectorXf& dst);
+
+        void diagonal(Eigen::VectorXf& v,Eigen::SparseMatrix<float>& mat)
+        {
+            mat = Eigen::SparseMatrix<float>(v.size(),v.size());
+            for (int i = 0; i < int(v.size()); i++)
+            {
+                mat.insert(i,i) = v(i);
+            }
+        }
+
+
+
+    private:
+
+        int npixels;
+        int nvertices;
+        int dim;
+        int cols;
+        int rows;
+        std::vector<int> splat_idx;
+        std::vector<std::pair<int, int> > blur_idx;
+        Eigen::VectorXf m;
+        Eigen::VectorXf n;
+        Eigen::SparseMatrix<float, Eigen::ColMajor> blurs;
+        Eigen::SparseMatrix<float, Eigen::ColMajor> S;
+        Eigen::SparseMatrix<float, Eigen::ColMajor> Dn;
+        Eigen::SparseMatrix<float, Eigen::ColMajor> Dm;
+
+        struct grid_params
+        {
+            float spatialSigma;
+            float lumaSigma;
+            float chromaSigma;
+            grid_params()
+            {
+                spatialSigma = 8.0;
+                lumaSigma = 4.0;
+                chromaSigma = 4.0;
+            }
+        };
+
+        struct bs_params
+        {
+            float lam;
+            float A_diag_min;
+            float cg_tol;
+            int cg_maxiter;
+            bs_params()
+            {
+                lam = 128.0;
+                A_diag_min = 1e-5;
+                cg_tol = 1e-5;
+                cg_maxiter = 25;
+            }
+        };
+
+        grid_params grid_param;
+        bs_params bs_param;
+
+    };
+
+
+
+    void FastBilateralSolverFilterImpl::init(cv::Mat& reference, double sigma_spatial, double sigma_luma, double sigma_chroma, int num_iter, double max_tol)
+    {
+
+        bs_param.cg_maxiter = num_iter;
+        bs_param.cg_tol = max_tol;
+
+        if(reference.channels()==1)
+        {
+            dim = 3;
+            cols = reference.cols;
+            rows = reference.rows;
+            npixels = cols*rows;
+            long long hash_vec[3];
+            for (int i = 0; i < 3; ++i)
+            hash_vec[i] = static_cast<long long>(std::pow(255, i));
+
+            mapId hashed_coords;
+#if __cplusplus <= 199711L
+#else
+            hashed_coords.reserve(cols*rows);
+#endif
+
+            const unsigned char* pref = (const unsigned char*)reference.data;
+            int vert_idx = 0;
+            int pix_idx = 0;
+
+            // construct Splat(Slice) matrices
+            splat_idx.resize(npixels);
+            for (int y = 0; y < rows; ++y)
+            {
+                for (int x = 0; x < cols; ++x)
+                {
+                    long long coord[3];
+                    coord[0] = int(x / sigma_spatial);
+                    coord[1] = int(y / sigma_spatial);
+                    coord[2] = int(pref[0] / sigma_luma);
+
+                    // convert the coordinate to a hash value
+                    long long hash_coord = 0;
+                    for (int i = 0; i < 3; ++i)
+                        hash_coord += coord[i] * hash_vec[i];
+
+                    // pixels whom are alike will have the same hash value.
+                    // We only want to keep a unique list of hash values, therefore make sure we only insert
+                    // unique hash values.
+                    mapId::iterator it = hashed_coords.find(hash_coord);
+                    if (it == hashed_coords.end())
+                    {
+                        hashed_coords.insert(std::pair<long long, int>(hash_coord, vert_idx));
+                        splat_idx[pix_idx] = vert_idx;
+                        ++vert_idx;
+                    }
+                    else
+                    {
+                        splat_idx[pix_idx] = it->second;
+                    }
+
+                    pref += 1; // skip 1 bytes (y)
+                    ++pix_idx;
+                }
+            }
+            nvertices = hashed_coords.size();
+
+            // construct Blur matrices
+            Eigen::VectorXf ones_nvertices = Eigen::VectorXf::Ones(nvertices);
+            Eigen::VectorXf ones_npixels = Eigen::VectorXf::Ones(npixels);
+            diagonal(ones_nvertices,blurs);
+            blurs *= 10;
+            for(int offset = -1; offset <= 1;++offset)
+            {
+                if(offset == 0) continue;
+                for (int i = 0; i < dim; ++i)
+                {
+                    Eigen::SparseMatrix<float, Eigen::ColMajor> blur_temp(hashed_coords.size(), hashed_coords.size());
+                    blur_temp.reserve(Eigen::VectorXi::Constant(nvertices,6));
+                    long long offset_hash_coord = offset * hash_vec[i];
+                    for (mapId::iterator it = hashed_coords.begin(); it != hashed_coords.end(); ++it)
+                    {
+                        long long neighb_coord = it->first + offset_hash_coord;
+                        mapId::iterator it_neighb = hashed_coords.find(neighb_coord);
+                        if (it_neighb != hashed_coords.end())
+                        {
+                            blur_temp.insert(it->second,it_neighb->second) = 1.0f;
+                            blur_idx.push_back(std::pair<int,int>(it->second, it_neighb->second));
+                        }
+                    }
+                    blurs += blur_temp;
+                }
+            }
+            blurs.finalize();
+
+            //bistochastize
+            int maxiter = 10;
+            n = ones_nvertices;
+            m = Eigen::VectorXf::Zero(nvertices);
+            for (int i = 0; i < int(splat_idx.size()); i++)
+            {
+                m(splat_idx[i]) += 1.0f;
+            }
+
+            Eigen::VectorXf bluredn(nvertices);
+
+            for (int i = 0; i < maxiter; i++)
+            {
+                Blur(n,bluredn);
+                n = ((n.array()*m.array()).array()/bluredn.array()).array().sqrt();
+            }
+            Blur(n,bluredn);
+
+            m = n.array() * (bluredn).array();
+            diagonal(m,Dm);
+            diagonal(n,Dn);
+
+        }
+        else
+        {
+            dim = 5;
+            cv::Mat reference_yuv;
+            cv::cvtColor(reference, reference_yuv, COLOR_BGR2YCrCb);
+
+            cols = reference_yuv.cols;
+            rows = reference_yuv.rows;
+            npixels = cols*rows;
+            long long hash_vec[5];
+            for (int i = 0; i < 5; ++i)
+            hash_vec[i] = static_cast<long long>(std::pow(255, i));
+
+            mapId hashed_coords;
+#if __cplusplus <= 199711L
+#else
+            hashed_coords.reserve(cols*rows);
+#endif
+
+            const unsigned char* pref = (const unsigned char*)reference_yuv.data;
+            int vert_idx = 0;
+            int pix_idx = 0;
+
+            // construct Splat(Slice) matrices
+            splat_idx.resize(npixels);
+            for (int y = 0; y < rows; ++y)
+            {
+                for (int x = 0; x < cols; ++x)
+                {
+                    long long coord[5];
+                    coord[0] = int(x / sigma_spatial);
+                    coord[1] = int(y / sigma_spatial);
+                    coord[2] = int(pref[0] / sigma_luma);
+                    coord[3] = int(pref[1] / sigma_chroma);
+                    coord[4] = int(pref[2] / sigma_chroma);
+
+                    // convert the coordinate to a hash value
+                    long long hash_coord = 0;
+                    for (int i = 0; i < 5; ++i)
+                        hash_coord += coord[i] * hash_vec[i];
+
+                    // pixels whom are alike will have the same hash value.
+                    // We only want to keep a unique list of hash values, therefore make sure we only insert
+                    // unique hash values.
+                    mapId::iterator it = hashed_coords.find(hash_coord);
+                    if (it == hashed_coords.end())
+                    {
+                        hashed_coords.insert(std::pair<long long, int>(hash_coord, vert_idx));
+                        splat_idx[pix_idx] = vert_idx;
+                        ++vert_idx;
+                    }
+                    else
+                    {
+                        splat_idx[pix_idx] = it->second;
+                    }
+
+                    pref += 3; // skip 3 bytes (y u v)
+                    ++pix_idx;
+                }
+            }
+            nvertices = hashed_coords.size();
+
+            // construct Blur matrices
+            Eigen::VectorXf ones_nvertices = Eigen::VectorXf::Ones(nvertices);
+            Eigen::VectorXf ones_npixels = Eigen::VectorXf::Ones(npixels);
+            diagonal(ones_nvertices,blurs);
+            blurs *= 10;
+            for(int offset = -1; offset <= 1;++offset)
+            {
+                if(offset == 0) continue;
+                for (int i = 0; i < dim; ++i)
+                {
+                    Eigen::SparseMatrix<float, Eigen::ColMajor> blur_temp(hashed_coords.size(), hashed_coords.size());
+                    blur_temp.reserve(Eigen::VectorXi::Constant(nvertices,6));
+                    long long offset_hash_coord = offset * hash_vec[i];
+                    for (mapId::iterator it = hashed_coords.begin(); it != hashed_coords.end(); ++it)
+                    {
+                        long long neighb_coord = it->first + offset_hash_coord;
+                        mapId::iterator it_neighb = hashed_coords.find(neighb_coord);
+                        if (it_neighb != hashed_coords.end())
+                        {
+                            blur_temp.insert(it->second,it_neighb->second) = 1.0f;
+                            blur_idx.push_back(std::pair<int,int>(it->second, it_neighb->second));
+                        }
+                    }
+                    blurs += blur_temp;
+                }
+            }
+            blurs.finalize();
+
+
+            //bistochastize
+            int maxiter = 10;
+            n = ones_nvertices;
+            m = Eigen::VectorXf::Zero(nvertices);
+            for (int i = 0; i < int(splat_idx.size()); i++)
+            {
+                m(splat_idx[i]) += 1.0f;
+            }
+
+            Eigen::VectorXf bluredn(nvertices);
+
+            for (int i = 0; i < maxiter; i++)
+            {
+                Blur(n,bluredn);
+                n = ((n.array()*m.array()).array()/bluredn.array()).array().sqrt();
+            }
+            Blur(n,bluredn);
+
+            m = n.array() * (bluredn).array();
+            diagonal(m,Dm);
+            diagonal(n,Dn);
+        }
+    }
+
+    void FastBilateralSolverFilterImpl::Splat(Eigen::VectorXf& input, Eigen::VectorXf& output)
+    {
+        output.setZero();
+        for (int i = 0; i < int(splat_idx.size()); i++)
+        {
+            output(splat_idx[i]) += input(i);
+        }
+
+    }
+
+    void FastBilateralSolverFilterImpl::Blur(Eigen::VectorXf& input, Eigen::VectorXf& output)
+    {
+        output.setZero();
+        output = input * 10;
+        for (int i = 0; i < int(blur_idx.size()); i++)
+        {
+            output(blur_idx[i].first) += input(blur_idx[i].second);
+        }
+    }
+
+
+    void FastBilateralSolverFilterImpl::Slice(Eigen::VectorXf& input, Eigen::VectorXf& output)
+    {
+        output.setZero();
+        for (int i = 0; i < int(splat_idx.size()); i++)
+        {
+            output(i) = input(splat_idx[i]);
+        }
+    }
+
+
+    void FastBilateralSolverFilterImpl::solve(cv::Mat& target,
+               cv::Mat& confidence,
+               cv::Mat& output)
+    {
+
+        Eigen::SparseMatrix<float, Eigen::ColMajor> M(nvertices,nvertices);
+        Eigen::SparseMatrix<float, Eigen::ColMajor> A_data(nvertices,nvertices);
+        Eigen::SparseMatrix<float, Eigen::ColMajor> A(nvertices,nvertices);
+        Eigen::VectorXf b(nvertices);
+        Eigen::VectorXf y(nvertices);
+        Eigen::VectorXf y0(nvertices);
+        Eigen::VectorXf y1(nvertices);
+        Eigen::VectorXf w_splat(nvertices);
+
+        Eigen::VectorXf x(npixels);
+        Eigen::VectorXf w(npixels);
+
+        if(target.depth() == CV_16S)
+        {
+            const int16_t *pft = reinterpret_cast<const int16_t*>(target.data);
+            for (int i = 0; i < npixels; i++)
+            {
+                x(i) = (cv::saturate_cast<float>(pft[i])+32768.0f)/65535.0f;
+            }
+        }
+        else if(target.depth() == CV_8U)
+        {
+            const uchar *pft = reinterpret_cast<const uchar*>(target.data);
+            for (int i = 0; i < npixels; i++)
+            {
+                x(i) = cv::saturate_cast<float>(pft[i])/255.0f;
+            }
+        }
+        else if(confidence.depth() == CV_32F)
+        {
+            const float *pft = reinterpret_cast<const float*>(target.data);
+            for (int i = 0; i < npixels; i++)
+            {
+                x(i) = pft[i];
+            }
+        }
+
+        if(confidence.depth() == CV_8U)
+        {
+            const uchar *pfc = reinterpret_cast<const uchar*>(confidence.data);
+            for (int i = 0; i < npixels; i++)
+            {
+                w(i) = cv::saturate_cast<float>(pfc[i])/255.0f;
+            }
+        }
+        else if(confidence.depth() == CV_32F)
+        {
+            const float *pfc = reinterpret_cast<const float*>(confidence.data);
+            for (int i = 0; i < npixels; i++)
+            {
+                w(i) = pfc[i];
+            }
+        }
+
+        //construct A
+        Splat(w,w_splat);
+
+        diagonal(w_splat,A_data);
+        A = bs_param.lam * (Dm - Dn * (blurs*Dn)) + A_data ;
+
+        //construct b
+        b.setZero();
+        for (int i = 0; i < int(splat_idx.size()); i++)
+        {
+            b(splat_idx[i]) += x(i) * w(i);
+        }
+
+        //construct guess for y
+        y0.setZero();
+        for (int i = 0; i < int(splat_idx.size()); i++)
+        {
+            y0(splat_idx[i]) += x(i);
+        }
+        y1.setZero();
+        for (int i = 0; i < int(splat_idx.size()); i++)
+        {
+            y1(splat_idx[i]) += 1.0f;
+        }
+        for (int i = 0; i < nvertices; i++)
+        {
+            y0(i) = y0(i)/y1(i);
+        }
+
+
+        // solve Ay = b
+        Eigen::ConjugateGradient<Eigen::SparseMatrix<float>, Eigen::Lower|Eigen::Upper> cg;
+        cg.compute(A);
+        cg.setMaxIterations(bs_param.cg_maxiter);
+        cg.setTolerance(bs_param.cg_tol);
+        // y = cg.solve(b);
+        y = cg.solveWithGuess(b,y0);
+        std::cout << "#iterations:     " << cg.iterations() << std::endl;
+        std::cout << "estimated error: " << cg.error()      << std::endl;
+
+        //slice
+        if(target.depth() == CV_16S)
+        {
+            int16_t *pftar = (int16_t*) output.data;
+            for (int i = 0; i < int(splat_idx.size()); i++)
+            {
+                pftar[i] = cv::saturate_cast<ushort>(y(splat_idx[i]) * 65535.0f - 32768.0f);
+            }
+        }
+        else if (target.depth() == CV_8U)
+        {
+            uchar *pftar = (uchar*) output.data;
+            for (int i = 0; i < int(splat_idx.size()); i++)
+            {
+                pftar[i] = cv::saturate_cast<uchar>(y(splat_idx[i]) * 255.0f);
+            }
+        }
+        else
+        {
+            float *pftar = (float*)(output.data);
+            for (int i = 0; i < int(splat_idx.size()); i++)
+            {
+                pftar[i] = y(splat_idx[i]);
+            }
+        }
+
+
+    }
+
+
+////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////
+Ptr<FastBilateralSolverFilter> createFastBilateralSolverFilter(InputArray guide, double sigma_spatial, double sigma_luma, double sigma_chroma, int num_iter, double max_tol)
+{
+    return Ptr<FastBilateralSolverFilter>(FastBilateralSolverFilterImpl::create(guide, sigma_spatial, sigma_luma, sigma_chroma, num_iter, max_tol));
+}
+
+void fastBilateralSolverFilter(InputArray guide, InputArray src, InputArray confidence, OutputArray dst, double sigma_spatial, double sigma_luma, double sigma_chroma, int num_iter, double max_tol)
+{
+    Ptr<FastBilateralSolverFilter> fbs = createFastBilateralSolverFilter(guide, sigma_spatial, sigma_luma, sigma_chroma, num_iter, max_tol);
+    fbs->filter(src, confidence, dst);
+}
+
+}
+
+}
+
+#else
+
+namespace cv
+{
+namespace ximgproc
+{
+
+Ptr<FastBilateralSolverFilter> createFastBilateralSolverFilter(InputArray, double, double, double, int, double)
+{
+    CV_Error(Error::StsNotImplemented, "createFastBilateralSolverFilter : needs to be compiled with EIGEN");
+}
+
+void fastBilateralSolverFilter(InputArray, InputArray, InputArray, OutputArray, double, double, double, int, double)
+{
+    CV_Error(Error::StsNotImplemented, "fastBilateralSolverFilter : needs to be compiled with EIGEN");
+}
+
+
+}
+
+}
+
+#endif // HAVE_EIGEN

modules/ximgproc/test/test_fbs_filter.cpp

+/*
+ *  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 "test_precomp.hpp"
+
+#ifdef HAVE_EIGEN
+
+namespace opencv_test { namespace {
+
+using namespace std;
+using namespace cv;
+using namespace cv::ximgproc;
+
+static string getDataDir()
+{
+    return cvtest::TS::ptr()->get_data_path();
+}
+
+CV_ENUM(SrcTypes, CV_8UC1, CV_8UC3, CV_8UC4, CV_16SC1, CV_16SC3, CV_32FC1);
+CV_ENUM(GuideTypes, CV_8UC1, CV_8UC3)
+typedef tuple<Size, SrcTypes, GuideTypes> FBSParams;
+typedef TestWithParam<FBSParams> FastBilateralSolverTest;
+
+TEST(FastBilateralSolverTest, SplatSurfaceAccuracy)
+{
+    RNG rnd(0);
+    int chanLut[] = {1,3,4};
+
+    for (int i = 0; i < 5; i++)
+    {
+        Size sz(rnd.uniform(512, 1024), rnd.uniform(512, 1024));
+
+        int guideCn = rnd.uniform(0, 2); // 1 or 3 channels
+        Mat guide(sz, CV_MAKE_TYPE(CV_8U, chanLut[guideCn]));
+        randu(guide, 0, 255);
+
+        Scalar surfaceValue;
+        int srcCn = rnd.uniform(0, 3); // 1, 3 or 4 channels
+        rnd.fill(surfaceValue, RNG::UNIFORM, 0, 255);
+        Mat src(sz, CV_MAKE_TYPE(CV_16S, chanLut[srcCn]), surfaceValue);
+        Mat confidence(sz, CV_MAKE_TYPE(CV_8U, 1), 255);
+
+        double sigma_spatial = rnd.uniform(4.0, 40.0);
+        double sigma_luma = rnd.uniform(4.0, 40.0);
+        double sigma_chroma  = rnd.uniform(4.0, 40.0);
+
+        Mat res;
+        fastBilateralSolverFilter(guide, src, confidence, res, sigma_spatial, sigma_luma, sigma_chroma);
+
+        // When filtering a constant image we should get the same image:
+        double normL1 = cvtest::norm(src, res, NORM_L1)/src.total()/src.channels();
+        EXPECT_LE(normL1, 1.0);
+    }
+}
+
+TEST(FastBilateralSolverTest, ReferenceAccuracy)
+{
+    string dir = getDataDir() + "cv/edgefilter";
+
+    Mat src = imread(dir + "/kodim23.png");
+    Mat ref = imread(dir + "/fgs/kodim23_lambda=1000_sigma=10.png");
+    Mat confidence(src.size(), CV_MAKE_TYPE(CV_8U, 1), 255);
+
+    ASSERT_FALSE(src.empty());
+    ASSERT_FALSE(ref.empty());
+
+    Mat res;
+    fastBilateralSolverFilter(src,src,confidence,res, 16.0, 16.0, 16.0);
+
+    double totalMaxError = 1.0/64.0*src.total()*src.channels();
+
+    EXPECT_LE(cvtest::norm(res, ref, NORM_L2), totalMaxError);
+    EXPECT_LE(cvtest::norm(res, ref, NORM_INF), 100);
+}
+
+INSTANTIATE_TEST_CASE_P(FullSet, FastBilateralSolverTest,Combine(Values(szODD, szQVGA), SrcTypes::all(), GuideTypes::all()));
+
+}
+}
+
+#endif //HAVE_EIGEN