stereocsbp.cl

/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  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
//
// Copyright (C) 2010-2012, Multicoreware, Inc., all rights reserved.
// Copyright (C) 2010-2012, Institute Of Software Chinese Academy Of Science, all rights reserved.
// Copyright (C) 2010-2012, Advanced Micro Devices, Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// @Authors
//    Jia Haipeng, jiahaipeng95@gmail.com
//    Jin Ma, jin@multicorewareinc.com
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other oclMaterials provided with the distribution.
//
//   * The name of the copyright holders may not 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 the Intel Corporation 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.
//
//M*/


#ifndef FLT_MAX
#define FLT_MAX  CL_FLT_MAX
#endif

#ifndef SHRT_MAX
#define SHRT_MAX  CL_SHORT_MAX
#endif

    
///////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////get_first_k_initial_global//////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////
__kernel void get_first_k_initial_global_0(__global short *data_cost_selected_, __global short *selected_disp_pyr,
                                           __global short *ctemp, int h, int w, int nr_plane,
                                           int cmsg_step1, int cdisp_step1, int cndisp)
{
    int x = get_global_id(0);
    int y = get_global_id(1);

    if (y < h && x < w)
    {
        __global short *selected_disparity = selected_disp_pyr      + y * cmsg_step1 + x;
        __global short *data_cost_selected = data_cost_selected_    + y * cmsg_step1 + x;
        __global short *data_cost          = ctemp + y * cmsg_step1 + x;

        for(int i = 0; i < nr_plane; i++)
        {
            short minimum = SHRT_MAX;
            int id = 0;
           
            for(int d = 0; d < cndisp; d++)
            {
                short cur = data_cost[d * cdisp_step1];
                if(cur < minimum)
                {
                    minimum = cur;
                    id = d;
                }
            }
                
            data_cost_selected[i  * cdisp_step1] = minimum;
            selected_disparity[i  * cdisp_step1] = id;
            data_cost         [id * cdisp_step1] = SHRT_MAX;
        }
    }
}
__kernel void get_first_k_initial_global_1(__global  float *data_cost_selected_, __global float *selected_disp_pyr,
                                           __global  float *ctemp, int h, int w, int nr_plane,
                                           int cmsg_step1, int cdisp_step1, int cndisp)
{
    int x = get_global_id(0);
    int y = get_global_id(1);

    if (y < h && x < w)
    {
        __global   float *selected_disparity = selected_disp_pyr      + y * cmsg_step1 + x;
        __global   float *data_cost_selected = data_cost_selected_    + y * cmsg_step1 + x;
        __global   float *data_cost          = ctemp + y * cmsg_step1 + x;

        for(int i = 0; i < nr_plane; i++)
        {
            float minimum = FLT_MAX;
            int id = 0;
           
            for(int d = 0; d < cndisp; d++)
            {
                float cur = data_cost[d * cdisp_step1];
                if(cur < minimum)
                {
                    minimum = cur;
                    id = d;
                }
            }
                
            data_cost_selected[i  * cdisp_step1] = minimum;
            selected_disparity[i  * cdisp_step1] = id;
            data_cost         [id * cdisp_step1] = FLT_MAX;
        }
    }
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////get_first_k_initial_local////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////
__kernel void get_first_k_initial_local_0(__global  short *data_cost_selected_, __global short *selected_disp_pyr, 
                                          __global  short *ctemp,int h, int w, int nr_plane,
                                          int cmsg_step1, int cdisp_step1, int cndisp)
{
    int x = get_global_id(0);
    int y = get_global_id(1);

    if (y < h && x < w)
    {
        __global short *selected_disparity = selected_disp_pyr   + y * cmsg_step1 + x;
        __global short *data_cost_selected = data_cost_selected_ + y * cmsg_step1 + x;
        __global short *data_cost = ctemp + y * cmsg_step1 + x;

        int nr_local_minimum = 0;

        short prev = data_cost[0 * cdisp_step1];
        short cur  = data_cost[1 * cdisp_step1];
        short next = data_cost[2 * cdisp_step1];
            
        for (int d = 1; d < cndisp - 1 && nr_local_minimum < nr_plane; d++)
        {
                
            if (cur < prev && cur < next)
            {
                data_cost_selected[nr_local_minimum * cdisp_step1] = cur;
                selected_disparity[nr_local_minimum * cdisp_step1] = d;
                data_cost[d * cdisp_step1] = SHRT_MAX;

                nr_local_minimum++;
            }
                
            prev = cur;
            cur = next;
            next = data_cost[(d + 1) * cdisp_step1];
        }

        for (int i = nr_local_minimum; i < nr_plane; i++)
        {
            short minimum = SHRT_MAX;
            int id = 0;
                
            for (int d = 0; d < cndisp; d++)
            {
                cur = data_cost[d * cdisp_step1];
                if (cur < minimum)
                {
                    minimum = cur;
                    id = d;
                }
            }
                
            data_cost_selected[i * cdisp_step1] = minimum;
            selected_disparity[i * cdisp_step1] = id;
            data_cost[id * cdisp_step1] = SHRT_MAX;
        }
    }
}

__kernel void get_first_k_initial_local_1(__global float *data_cost_selected_, __global float *selected_disp_pyr, 
                                          __global float *ctemp,int h, int w, int nr_plane,
                                          int cmsg_step1,  int cdisp_step1, int cndisp)
{
    int x = get_global_id(0);
    int y = get_global_id(1);

    if (y < h && x < w)
    {
        __global float *selected_disparity = selected_disp_pyr   + y * cmsg_step1 + x;
        __global float *data_cost_selected = data_cost_selected_ + y * cmsg_step1 + x;
        __global float *data_cost = ctemp + y * cmsg_step1 + x;

        int nr_local_minimum = 0;

        float prev = data_cost[0 * cdisp_step1];
        float cur  = data_cost[1 * cdisp_step1];
        float next = data_cost[2 * cdisp_step1];
            
        for (int d = 1; d < cndisp - 1 && nr_local_minimum < nr_plane; d++)
        {
            if (cur < prev && cur < next)
            {
                data_cost_selected[nr_local_minimum * cdisp_step1] = cur;
                selected_disparity[nr_local_minimum * cdisp_step1] = d;
                data_cost[d * cdisp_step1] = FLT_MAX ;

                nr_local_minimum++;
            }
                
            prev = cur;
            cur = next;
            next = data_cost[(d + 1) * cdisp_step1];
        }
        for (int i = nr_local_minimum; i < nr_plane; i++)
        {
            float minimum = FLT_MAX;
            int id = 0;
                
            for (int d = 0; d < cndisp; d++)
            {
                cur = data_cost[d * cdisp_step1];
                if (cur < minimum)
                {
                    minimum = cur;
                    id = d;
                }
            }
                
            data_cost_selected[i * cdisp_step1] = minimum;
            selected_disparity[i * cdisp_step1] = id;
            data_cost[id * cdisp_step1] = FLT_MAX;
        }
    }
}

///////////////////////////////////////////////////////////////
/////////////////////// init data cost ////////////////////////
///////////////////////////////////////////////////////////////
float compute_3(__global uchar* left, __global uchar* right,
                 float cdata_weight,  float cmax_data_term)
{
    float tb = 0.114f * abs((int)left[0] - right[0]);
    float tg = 0.587f * abs((int)left[1] - right[1]);
    float tr = 0.299f * abs((int)left[2] - right[2]);
   
    return fmin(cdata_weight * (tr + tg + tb), cdata_weight * cmax_data_term);
}
float compute_1(__global uchar* left, __global uchar* right, 
                 float cdata_weight,  float cmax_data_term)
{
    return fmin(cdata_weight * abs((int)*left - (int)*right), cdata_weight * cmax_data_term);
}
short round_short(float v){
    return convert_short_sat_rte(v); 
}
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////init_data_cost///////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
__kernel void init_data_cost_0(__global short *ctemp, __global uchar *cleft, __global uchar *cright, 
                               int h, int w, int level, int channels,
                               int cmsg_step1, float cdata_weight, float cmax_data_term, int cdisp_step1, 
                               int cth, int cimg_step, int cndisp)
{
    int x = get_global_id(0);
    int y = get_global_id(1);
   
    if (y < h && x < w)
    {
        int y0 = y << level;
        int yt = (y + 1) << level;
       
        int x0 = x << level;
        int xt = (x + 1) << level;

        __global short *data_cost = ctemp + y * cmsg_step1 + x;
       
        for(int d = 0; d < cndisp; ++d)
        {
            float val = 0.0f;
            for(int yi = y0; yi < yt; yi++)
            {
                for(int xi = x0; xi < xt; xi++)
                {
                    int xr = xi - d;
                    if(d < cth || xr < 0)
                        val += cdata_weight * cmax_data_term;
                    else
                    {
                        __global uchar *lle = cleft  + yi * cimg_step + xi * channels;
                        __global uchar *lri = cright + yi * cimg_step + xr * channels;

                        if(channels == 1)
                            val += compute_1(lle, lri, cdata_weight, cmax_data_term);
                        else
                            val += compute_3(lle, lri, cdata_weight, cmax_data_term);
                    }
                }
            }
            data_cost[cdisp_step1 * d] = round_short(val);
        }
    }
}
__kernel void init_data_cost_1(__global float *ctemp, __global uchar *cleft, __global uchar *cright, 
                               int h, int w, int level, int channels,
                               int cmsg_step1, float cdata_weight, float cmax_data_term, int cdisp_step1, 
                               int cth, int cimg_step, int cndisp)
{
    int x = get_global_id(0);
    int y = get_global_id(1);
   
    if (y < h && x < w)
    {
        int y0 = y << level;
        int yt = (y + 1) << level;
       
        int x0 = x << level;
        int xt = (x + 1) << level;

        __global float *data_cost = ctemp + y * cmsg_step1 + x;
       
        for(int d = 0; d < cndisp; ++d)
        {
            float val = 0.0f;
            for(int yi = y0; yi < yt; yi++)
            {
                for(int xi = x0; xi < xt; xi++)
                {
                    int xr = xi - d;
                    if(d < cth || xr < 0)
                        val += cdata_weight * cmax_data_term;
                    else
                    {
                        __global uchar* lle = cleft  + yi * cimg_step + xi * channels;
                        __global uchar* lri = cright + yi * cimg_step + xr * channels;

                        if(channels == 1)
                            val += compute_1(lle, lri, cdata_weight, cmax_data_term);
                        else
                            val += compute_3(lle, lri, cdata_weight, cmax_data_term);
                    }
                }
            }
            data_cost[cdisp_step1 * d] = val;
        }
    }
}
////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////init_data_cost_reduce//////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////
__kernel void init_data_cost_reduce_0(__global short *ctemp, __global uchar *cleft, __global uchar *cright,
                                      __local float *smem, int level, int rows, int cols, int h, int winsz, int channels,
                                      int cndisp,int cimg_step, float cdata_weight, float cmax_data_term, int cth, 
                                      int cdisp_step1, int cmsg_step1)
{
    int x_out = get_group_id(0);
    int y_out = get_group_id(1) % h;
    //int d = (blockIdx.y / h) * blockDim.z + threadIdx.z;
    int d = (get_group_id(1) / h ) * get_local_size(2) + get_local_id(2); 

    int tid = get_local_id(0);

    if (d < cndisp)
    {
        int x0 = x_out << level;
        int y0 = y_out << level;
       
        int len = min(y0 + winsz, rows) - y0;
       
        float val = 0.0f;
        if (x0 + tid < cols)
        {
            if (x0 + tid - d < 0 || d < cth)
                val = cdata_weight * cmax_data_term * len;
            else
            {
                __global uchar* lle =  cleft + y0 * cimg_step + channels * (x0 + tid    );
                __global uchar* lri = cright + y0 * cimg_step + channels * (x0 + tid - d);
               
                for(int y = 0; y < len; ++y)
                {
                    if(channels == 1)
                        val += compute_1(lle, lri, cdata_weight, cmax_data_term);
                    else
                        val += compute_3(lle, lri, cdata_weight, cmax_data_term);

                    lle += cimg_step;
                    lri += cimg_step;
                }
            }
        }
       
        __local float* dline = smem + winsz * get_local_id(2);
       
        dline[tid] = val;

        barrier(CLK_LOCAL_MEM_FENCE);

        if (winsz >= 256) { if (tid < 128) { dline[tid] += dline[tid + 128]; } barrier(CLK_LOCAL_MEM_FENCE); }
        if (winsz >= 128) { if (tid <  64) { dline[tid] += dline[tid + 64]; }  barrier(CLK_LOCAL_MEM_FENCE); }

			  __local volatile float* vdline = smem + winsz * get_local_id(2);

        if (winsz >= 64) if (tid < 32) vdline[tid] += vdline[tid + 32];
        if (winsz >= 32) if (tid < 16) vdline[tid] += vdline[tid + 16];
        if (winsz >= 16) if (tid <  8) vdline[tid] += vdline[tid + 8];
        if (winsz >=  8) if (tid <  4) vdline[tid] += vdline[tid + 4];
        if (winsz >=  4) if (tid <  2) vdline[tid] += vdline[tid + 2];
        if (winsz >=  2) if (tid <  1) vdline[tid] += vdline[tid + 1];

        __global short* data_cost = ctemp + y_out * cmsg_step1 + x_out;

        if (tid == 0)
            data_cost[cdisp_step1 * d] = convert_short_sat_rte(dline[0]);
    }
}

__kernel void init_data_cost_reduce_1(__global float *ctemp, __global uchar *cleft, __global uchar *cright,
                                      __local float *smem, int level, int rows, int cols, int h, int winsz, int channels,
                                      int cndisp,int cimg_step, float cdata_weight, float cmax_data_term, int cth,
                                      int cdisp_step1, int cmsg_step1)
{
    int x_out = get_group_id(0);
    int y_out = get_group_id(1) % h;
    int d = (get_group_id(1) / h ) * get_local_size(2) + get_local_id(2); 

    int tid = get_local_id(0);

    if (d < cndisp)
    {
        int x0 = x_out << level;
        int y0 = y_out << level;
       
        int len = min(y0 + winsz, rows) - y0;
       
        float val = 0.0f;
        //float val = 528.0f;

        if (x0 + tid < cols)
        {
            if (x0 + tid - d < 0 || d < cth)
                val = cdata_weight * cmax_data_term * len;
            else
            {
                __global uchar* lle =  cleft + y0 * cimg_step + channels * (x0 + tid    );
                __global uchar* lri = cright + y0 * cimg_step + channels * (x0 + tid - d);
               
                for(int y = 0; y < len; ++y)
                {
                    if(channels == 1)
                        val += compute_1(lle, lri, cdata_weight, cmax_data_term);
                    else
                        val += compute_3(lle, lri, cdata_weight, cmax_data_term);

                    lle += cimg_step;
                    lri += cimg_step;
                }
            }
        }
    
        __local float* dline = smem + winsz * get_local_id(2);
      
        dline[tid] = val;

        barrier(CLK_LOCAL_MEM_FENCE);

        if (winsz >= 256) { if (tid < 128) { dline[tid] += dline[tid + 128]; } barrier(CLK_LOCAL_MEM_FENCE); }
        if (winsz >= 128) { if (tid <  64) { dline[tid] += dline[tid + 64]; }  barrier(CLK_LOCAL_MEM_FENCE); }

			  __local volatile float* vdline = smem + winsz * get_local_id(2);

        if (winsz >= 64) if (tid < 32) vdline[tid] += vdline[tid + 32];
        if (winsz >= 32) if (tid < 16) vdline[tid] += vdline[tid + 16];
        if (winsz >= 16) if (tid <  8) vdline[tid] += vdline[tid + 8];
        if (winsz >=  8) if (tid <  4) vdline[tid] += vdline[tid + 4];
        if (winsz >=  4) if (tid <  2) vdline[tid] += vdline[tid + 2];
        if (winsz >=  2) if (tid <  1) vdline[tid] += vdline[tid + 1];

        __global float *data_cost = ctemp + y_out * cmsg_step1 + x_out;

        if (tid == 0)
            data_cost[cdisp_step1 * d] =  dline[0];
    }
}

///////////////////////////////////////////////////////////////
////////////////////// compute data cost //////////////////////
///////////////////////////////////////////////////////////////
__kernel void compute_data_cost_0(__global const short *selected_disp_pyr, __global short *data_cost_, 
                                __global uchar *cleft, __global uchar *cright,
                                int h, int w, int level, int nr_plane, int channels,
                                int cmsg_step1, int cmsg_step2, int cdisp_step1, int cdisp_step2, float cdata_weight, 
                                float cmax_data_term, int cimg_step, int cth)
{

    int x = get_global_id(0);
    int y = get_global_id(1);

    if (y < h && x < w)
    {
        int y0 = y << level;
        int yt = (y + 1) << level;
       
        int x0 = x << level;
        int xt = (x + 1) << level;

        __global const short *selected_disparity = selected_disp_pyr + y/2 * cmsg_step2 + x/2;
        __global       short *data_cost          = data_cost_ + y * cmsg_step1 + x;

        for(int d = 0; d < nr_plane; d++)
        {
            float val = 0.0f;
            for(int yi = y0; yi < yt; yi++)
            {
                for(int xi = x0; xi < xt; xi++)
                {
                    int sel_disp = selected_disparity[d * cdisp_step2];
                    int xr = xi - sel_disp;
                        
                    if (xr < 0 || sel_disp < cth)
                        val += cdata_weight * cmax_data_term;
                      
                    else
                    {
                        __global uchar* left_x  = cleft + yi * cimg_step + xi * channels;
                        __global uchar* right_x = cright + yi * cimg_step + xr * channels;

                        if(channels == 1)
                            val += compute_1(left_x, right_x, cdata_weight, cmax_data_term);
                        else
                            val += compute_3(left_x, right_x, cdata_weight, cmax_data_term);
                    }
                }
            }
            data_cost[cdisp_step1 * d] = convert_short_sat_rte(val);
        }
    }
}
__kernel void compute_data_cost_1(__global const float *selected_disp_pyr, __global float *data_cost_, 
                                __global uchar *cleft, __global uchar *cright,
                                int h, int w, int level, int nr_plane, int channels,
                                int cmsg_step1, int cmsg_step2, int cdisp_step1, int cdisp_step2, float cdata_weight, 
                                float cmax_data_term, int cimg_step, int cth)
{

    int x = get_global_id(0);
    int y = get_global_id(1);

    if (y < h && x < w)
    {
        int y0 = y << level;
        int yt = (y + 1) << level;
       
        int x0 = x << level;
        int xt = (x + 1) << level;

        __global const float *selected_disparity = selected_disp_pyr + y/2 * cmsg_step2 + x/2;
        __global       float *data_cost          = data_cost_ + y * cmsg_step1 + x;

        for(int d = 0; d < nr_plane; d++)
        {
            float val = 0.0f;
            for(int yi = y0; yi < yt; yi++)
            {
                for(int xi = x0; xi < xt; xi++)
                {
                    int sel_disp = selected_disparity[d * cdisp_step2];
                    int xr = xi - sel_disp;
                        
                    if (xr < 0 || sel_disp < cth)
                        val += cdata_weight * cmax_data_term;
                    else
                    {
                        __global uchar* left_x  = cleft + yi * cimg_step + xi * channels;
                        __global uchar* right_x = cright + yi * cimg_step + xr * channels;

                        if(channels == 1)
                            val += compute_1(left_x, right_x, cdata_weight, cmax_data_term);
                        else
                            val += compute_3(left_x, right_x, cdata_weight, cmax_data_term);
                    }
                }
            }
            data_cost[cdisp_step1 * d] = val;
        }
    }
}
////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////compute_data_cost_reduce//////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
__kernel void compute_data_cost_reduce_0(__global const short* selected_disp_pyr, __global short* data_cost_, 
                                         __global uchar *cleft, __global uchar *cright,__local float *smem,
                                         int level, int rows, int cols, int h, int nr_plane, 
                                         int channels, int winsz,
                                         int cmsg_step1, int cmsg_step2, int cdisp_step1, int cdisp_step2, 
                                         float cdata_weight,  float cmax_data_term, int cimg_step,int cth)
    
{
    int x_out = get_group_id(0);
    int y_out = get_group_id(1) % h;
    int d = (get_group_id(1)/ h) * get_local_size(2) + get_local_id(2);
   
    int tid = get_local_id(0);
   
    __global const short* selected_disparity = selected_disp_pyr + y_out/2 * cmsg_step2 + x_out/2;
    __global short* data_cost = data_cost_ + y_out * cmsg_step1 + x_out;
   
    if (d < nr_plane)
    {
        int sel_disp = selected_disparity[d * cdisp_step2];
       
        int x0 = x_out << level;
        int y0 = y_out << level;
       
        int len = min(y0 + winsz, rows) - y0;
       
        float val = 0.0f;
        if (x0 + tid < cols)
        {
            if (x0 + tid - sel_disp < 0 || sel_disp < cth)
                val = cdata_weight * cmax_data_term * len;
            else
            {
                __global uchar* lle =  cleft + y0 * cimg_step + channels * (x0 + tid    );
                __global uchar* lri = cright + y0 * cimg_step + channels * (x0 + tid - sel_disp);
               
                for(int y = 0; y < len; ++y)
                {
                    if(channels == 1)
                        val += compute_1(lle, lri, cdata_weight, cmax_data_term);
                    else
                        val += compute_3(lle, lri, cdata_weight, cmax_data_term);

                    lle += cimg_step;
                    lri += cimg_step;
                }
            }
         }
        
         __local float* dline = smem + winsz * get_local_id(2);

         dline[tid] = val;
      }

      barrier(CLK_LOCAL_MEM_FENCE);
      if(d < nr_plane)
      {

        // if (winsz >= 256) { if (tid < 128) { dline[tid] += dline[tid + 128]; } barrier(CLK_LOCAL_MEM_FENCE); }
        //if (winsz >= 128) { if (tid <  64) { dline[tid] += dline[tid +  64]; } barrier(CLK_LOCAL_MEM_FENCE); }

         __local volatile float* vdline = smem + winsz * get_local_id(2);

         if (winsz >= 64) if (tid < 32) vdline[tid] += vdline[tid + 32];
         if (winsz >= 32) if (tid < 16) vdline[tid] += vdline[tid + 16];
         if (winsz >= 16) if (tid <  8) vdline[tid] += vdline[tid + 8];
         if (winsz >=  8) if (tid <  4) vdline[tid] += vdline[tid + 4];
         if (winsz >=  4) if (tid <  2) vdline[tid] += vdline[tid + 2];
         if (winsz >=  2) if (tid <  1) vdline[tid] += vdline[tid + 1];

         if (tid == 0)
             data_cost[cdisp_step1 * d] = convert_short_sat_rte(vdline[0]);
    }
}

__kernel void compute_data_cost_reduce_1(__global const float *selected_disp_pyr, __global float *data_cost_, 
                                         __global uchar *cleft, __global uchar *cright, __local float *smem,
                                         int level, int rows, int cols, int h, int nr_plane, 
                                         int channels, int winsz,
                                         int cmsg_step1, int cmsg_step2, int cdisp_step1,int cdisp_step2, float cdata_weight, 
                                         float cmax_data_term, int cimg_step, int cth)
    
{
    int x_out = get_group_id(0);
    int y_out = get_group_id(1) % h;
    int d = (get_group_id(1)/ h) * get_local_size(2) + get_local_id(2);
   
    int tid = get_local_id(0);
   
    __global const float *selected_disparity = selected_disp_pyr + y_out/2 * cmsg_step2 + x_out/2;
    __global float *data_cost = data_cost_ + y_out * cmsg_step1 + x_out;
   
    if (d < nr_plane)
    {
        int sel_disp = selected_disparity[d * cdisp_step2];
       
        int x0 = x_out << level;
        int y0 = y_out << level;
       
        int len = min(y0 + winsz, rows) - y0;
       
        float val = 0.0f;
        if (x0 + tid < cols)
        {
            if (x0 + tid - sel_disp < 0 || sel_disp < cth)
                val = cdata_weight * cmax_data_term * len;
            else
            {
                __global uchar* lle =  cleft + y0 * cimg_step + channels * (x0 + tid    );
                __global uchar* lri = cright + y0 * cimg_step + channels * (x0 + tid - sel_disp);
               
                for(int y = 0; y < len; ++y)
                {
                    if(channels == 1)
                        val += compute_1(lle, lri, cdata_weight, cmax_data_term);
                    else
                        val += compute_3(lle, lri, cdata_weight, cmax_data_term);

                    lle += cimg_step;
                    lri += cimg_step;
                }
            }
         }
        
         __local float* dline = smem + winsz * get_local_id(2);

         dline[tid] = val;
     }
         
     barrier(CLK_LOCAL_MEM_FENCE);
     if(d < nr_plane)
     {

         //if (winsz >= 256) { if (tid < 128) { dline[tid] += dline[tid + 128]; } barrier(CLK_LOCAL_MEM_FENCE); }
         //if (winsz >= 128) { if (tid <  64) { dline[tid] += dline[tid +  64]; } barrier(CLK_LOCAL_MEM_FENCE); }

         __local volatile float* vdline = smem + winsz * get_local_id(2);

         if (winsz >= 64) if (tid < 32) vdline[tid] += vdline[tid + 32];
         if (winsz >= 32) if (tid < 16) vdline[tid] += vdline[tid + 16];
         if (winsz >= 16) if (tid <  8) vdline[tid] += vdline[tid + 8];
         if (winsz >=  8) if (tid <  4) vdline[tid] += vdline[tid + 4];
         if (winsz >=  4) if (tid <  2) vdline[tid] += vdline[tid + 2];
         if (winsz >=  2) if (tid <  1) vdline[tid] += vdline[tid + 1];

         if (tid == 0)
             data_cost[cdisp_step1 * d] = vdline[0];
    }
}

///////////////////////////////////////////////////////////////
//////////////////////// init message /////////////////////////
///////////////////////////////////////////////////////////////
void get_first_k_element_increase_0(__global short* u_new, __global short *d_new, __global short *l_new, 
                                    __global short *r_new, __global const short *u_cur, __global const short *d_cur, 
                                    __global const short *l_cur, __global const short *r_cur, 
                                    __global short *data_cost_selected, __global short *disparity_selected_new, 
                                    __global short *data_cost_new, __global const short* data_cost_cur, 
                                    __global const short *disparity_selected_cur,
                                    int nr_plane, int nr_plane2,
                                    int cdisp_step1, int cdisp_step2)
{
    for(int i = 0; i < nr_plane; i++)
    {
        short minimum = SHRT_MAX;
        int id = 0;
        for(int j = 0; j < nr_plane2; j++)
        {
            short cur = data_cost_new[j * cdisp_step1];
            if(cur < minimum)
            {
                minimum = cur;
                id = j;
            }
        }

        data_cost_selected[i * cdisp_step1] = data_cost_cur[id * cdisp_step1];
        disparity_selected_new[i * cdisp_step1] = disparity_selected_cur[id * cdisp_step2];

        u_new[i * cdisp_step1] = u_cur[id * cdisp_step2];
        d_new[i * cdisp_step1] = d_cur[id * cdisp_step2];
        l_new[i * cdisp_step1] = l_cur[id * cdisp_step2];
        r_new[i * cdisp_step1] = r_cur[id * cdisp_step2];
       
        data_cost_new[id * cdisp_step1] = SHRT_MAX;
    }
}
void get_first_k_element_increase_1(__global float *u_new, __global float *d_new, __global float *l_new, 
                                    __global float *r_new, __global const float *u_cur, __global const float *d_cur, 
                                    __global const float *l_cur, __global const float *r_cur,
                                    __global float *data_cost_selected, __global float *disparity_selected_new, 
                                    __global float *data_cost_new, __global const float *data_cost_cur, 
                                    __global const float *disparity_selected_cur,
                                    int nr_plane, int nr_plane2,
                                    int cdisp_step1, int cdisp_step2)
{
    for(int i = 0; i < nr_plane; i++)
    {
        float minimum = FLT_MAX;
        int id = 0;

        for(int j = 0; j < nr_plane2; j++)
        {
            float cur = data_cost_new[j * cdisp_step1];
            if(cur < minimum)
            {
                minimum = cur;
                id = j;
            }
        }

        data_cost_selected[i * cdisp_step1] = data_cost_cur[id * cdisp_step1];
        disparity_selected_new[i * cdisp_step1] = disparity_selected_cur[id * cdisp_step2];

        u_new[i * cdisp_step1] = u_cur[id * cdisp_step2];
        d_new[i * cdisp_step1] = d_cur[id * cdisp_step2];
        l_new[i * cdisp_step1] = l_cur[id * cdisp_step2];
        r_new[i * cdisp_step1] = r_cur[id * cdisp_step2];   
        data_cost_new[id * cdisp_step1] = FLT_MAX;

    }
}
__kernel void init_message_0(__global short *u_new_, __global short *d_new_, __global short *l_new_,
                             __global short *r_new_, __global  short *u_cur_, __global const short *d_cur_, 
                             __global const short *l_cur_, __global const short *r_cur_, __global short *ctemp,
                             __global short *selected_disp_pyr_new, __global const short *selected_disp_pyr_cur,
                             __global short *data_cost_selected_, __global const short *data_cost_,
                             int h, int w, int nr_plane, int h2, int w2, int nr_plane2,
                             int cdisp_step1, int cdisp_step2, int cmsg_step1, int cmsg_step2)
{
    int x = get_global_id(0);
    int y = get_global_id(1);
   
    if (y < h && x < w)
    {
        __global const short *u_cur = u_cur_ + min(h2-1, y/2 + 1) * cmsg_step2 + x/2;
        __global const short *d_cur = d_cur_ + max(0, y/2 - 1)    * cmsg_step2 + x/2;
        __global const short *l_cur = l_cur_ + y/2                * cmsg_step2 + min(w2-1, x/2 + 1);
        __global const short *r_cur = r_cur_ + y/2                * cmsg_step2 + max(0, x/2 - 1);

        __global short *data_cost_new = ctemp + y * cmsg_step1 + x;
       
        __global const short *disparity_selected_cur = selected_disp_pyr_cur + y/2 * cmsg_step2 + x/2;
        __global const short *data_cost = data_cost_ + y * cmsg_step1 + x;

        for(int d = 0; d < nr_plane2; d++)
        {
            int idx2 = d * cdisp_step2;

            short val  = data_cost[d * cdisp_step1] + u_cur[idx2] + d_cur[idx2] + l_cur[idx2] + r_cur[idx2];
            data_cost_new[d * cdisp_step1] = val;
        }

        __global short *data_cost_selected = data_cost_selected_ + y * cmsg_step1 + x;
        __global short *disparity_selected_new = selected_disp_pyr_new + y * cmsg_step1 + x;

        __global short *u_new = u_new_ + y * cmsg_step1 + x;
        __global short *d_new = d_new_ + y * cmsg_step1 + x;
        __global short *l_new = l_new_ + y * cmsg_step1 + x;
        __global short *r_new = r_new_ + y * cmsg_step1 + x;

        u_cur = u_cur_ + y/2 * cmsg_step2 + x/2;
        d_cur = d_cur_ + y/2 * cmsg_step2 + x/2;
        l_cur = l_cur_ + y/2 * cmsg_step2 + x/2;
        r_cur = r_cur_ + y/2 * cmsg_step2 + x/2;

        get_first_k_element_increase_0(u_new, d_new, l_new, r_new, u_cur, d_cur, l_cur, r_cur,
                                       data_cost_selected, disparity_selected_new, data_cost_new,
                                       data_cost, disparity_selected_cur, nr_plane, nr_plane2,
                                       cdisp_step1, cdisp_step2);
    }
}
__kernel void init_message_1(__global float *u_new_, __global float *d_new_, __global float *l_new_,
                             __global float *r_new_, __global float *u_cur_, __global const float *d_cur_, 
                             __global const float *l_cur_, __global const float *r_cur_, __global float *ctemp,
                             __global float *selected_disp_pyr_new, __global const float *selected_disp_pyr_cur,
                             __global float *data_cost_selected_, __global const float *data_cost_,
                             int h, int w, int nr_plane, int h2, int w2, int nr_plane2,
                             int cdisp_step1, int cdisp_step2, int cmsg_step1, int cmsg_step2)
{
    int x = get_global_id(0);
    int y = get_global_id(1);
   
    if (y < h && x < w)
    {
        __global const float *u_cur = u_cur_ + min(h2-1, y/2 + 1) * cmsg_step2 + x/2;
        __global const float *d_cur = d_cur_ + max(0, y/2 - 1)    * cmsg_step2 + x/2;
        __global const float *l_cur = l_cur_ + y/2                * cmsg_step2 + min(w2-1, x/2 + 1);
        __global const float *r_cur = r_cur_ + y/2                * cmsg_step2 + max(0, x/2 - 1);

        __global float *data_cost_new = ctemp + y * cmsg_step1 + x;
       
        __global const float *disparity_selected_cur = selected_disp_pyr_cur + y/2 * cmsg_step2 + x/2;
        __global const float *data_cost = data_cost_ + y * cmsg_step1 + x;

        for(int d = 0; d < nr_plane2; d++)
        {
            int idx2 = d * cdisp_step2;

            float val  = data_cost[d * cdisp_step1] + u_cur[idx2] + d_cur[idx2] + l_cur[idx2] + r_cur[idx2];
            data_cost_new[d * cdisp_step1] = val;
        }

        __global float *data_cost_selected = data_cost_selected_ + y * cmsg_step1 + x;
        __global float *disparity_selected_new = selected_disp_pyr_new + y * cmsg_step1 + x;

        __global float *u_new = u_new_ + y * cmsg_step1 + x;
        __global float *d_new = d_new_ + y * cmsg_step1 + x;
        __global float *l_new = l_new_ + y * cmsg_step1 + x;
        __global float *r_new = r_new_ + y * cmsg_step1 + x;

        u_cur = u_cur_ + y/2 * cmsg_step2 + x/2;
        d_cur = d_cur_ + y/2 * cmsg_step2 + x/2;
        l_cur = l_cur_ + y/2 * cmsg_step2 + x/2;
        r_cur = r_cur_ + y/2 * cmsg_step2 + x/2;

        get_first_k_element_increase_1(u_new, d_new, l_new, r_new, u_cur, d_cur, l_cur, r_cur,
                                       data_cost_selected, disparity_selected_new, data_cost_new,
                                       data_cost, disparity_selected_cur, nr_plane, nr_plane2,
                                       cdisp_step1, cdisp_step2);
    }
}
///////////////////////////////////////////////////////////////
////////////////////  calc all iterations /////////////////////
///////////////////////////////////////////////////////////////
void message_per_pixel_0(__global const short *data, __global short *msg_dst, __global const short *msg1, 
                         __global const short *msg2, __global const short *msg3,
                         __global const short *dst_disp, __global const short *src_disp, 
                         int nr_plane, __global short *temp,
                         float cmax_disc_term, int cdisp_step1, float cdisc_single_jump)
{
    short minimum = SHRT_MAX;
    for(int d = 0; d < nr_plane; d++)     
    {
        int idx = d * cdisp_step1;
        short val  = data[idx] + msg1[idx] + msg2[idx] + msg3[idx];
       
        if(val < minimum)
            minimum = val;
           
        msg_dst[idx] = val;
    }
        
    float sum = 0;
    for(int d = 0; d < nr_plane; d++)
    {
        float cost_min = minimum + cmax_disc_term;
        short src_disp_reg = src_disp[d * cdisp_step1];
       
        for(int d2 = 0; d2 < nr_plane; d2++)
            cost_min = fmin(cost_min, (msg_dst[d2 * cdisp_step1] + 
                                       cdisc_single_jump * abs(dst_disp[d2 * cdisp_step1] - src_disp_reg)));

        temp[d * cdisp_step1] = convert_short_sat_rte(cost_min);
        sum += cost_min;
    }
    sum /= nr_plane;

    for(int d = 0; d < nr_plane; d++)
        msg_dst[d * cdisp_step1] = convert_short_sat_rte(temp[d * cdisp_step1] - sum);
}
void message_per_pixel_1(__global const float *data, __global float *msg_dst, __global const float *msg1, 
                         __global const float *msg2, __global const float *msg3,
                         __global const float *dst_disp, __global const float *src_disp, 
                         int nr_plane, __global float *temp,
                         float cmax_disc_term, int cdisp_step1, float cdisc_single_jump)
{
    float minimum = FLT_MAX;
    for(int d = 0; d < nr_plane; d++)     
    {
        int idx = d * cdisp_step1;
        float val  = data[idx] + msg1[idx] + msg2[idx] + msg3[idx];
       
        if(val < minimum)
            minimum = val;
           
        msg_dst[idx] = val;
    }
        
    float sum = 0;
    for(int d = 0; d < nr_plane; d++)
    {
        float cost_min = minimum + cmax_disc_term;
        float src_disp_reg = src_disp[d * cdisp_step1];
       
        for(int d2 = 0; d2 < nr_plane; d2++)
            cost_min = fmin(cost_min, (msg_dst[d2 * cdisp_step1] + 
                                       cdisc_single_jump * fabs(dst_disp[d2 * cdisp_step1] - src_disp_reg)));

        temp[d * cdisp_step1] = cost_min;
        sum += cost_min;
    }
    sum /= nr_plane;

    for(int d = 0; d < nr_plane; d++)
        msg_dst[d * cdisp_step1] = temp[d * cdisp_step1] - sum;
}
__kernel void compute_message_0(__global short *u_, __global short *d_, __global short *l_, __global short *r_, 
                                __global const short *data_cost_selected, __global const short *selected_disp_pyr_cur, 
                                __global short *ctemp, int h, int w, int nr_plane, int i, 
                                float cmax_disc_term, int cdisp_step1, int cmsg_step1, float cdisc_single_jump)
{
    int y = get_global_id(1);
    int x = ((get_global_id(0)) << 1) + ((y + i) & 1);
   
    if (y > 0 && y < h - 1 && x > 0 && x < w - 1)
    {
        __global const short *data = data_cost_selected + y * cmsg_step1 + x;
      
        __global short *u = u_ + y * cmsg_step1 + x;
        __global short *d = d_ + y * cmsg_step1 + x;
        __global short *l = l_ + y * cmsg_step1 + x;
        __global short *r = r_ + y * cmsg_step1 + x;
       
        __global const short *disp = selected_disp_pyr_cur + y * cmsg_step1 + x;

        __global short *temp = ctemp + y * cmsg_step1 + x;

        message_per_pixel_0(data, u, r - 1, u + cmsg_step1, l + 1, disp, disp - cmsg_step1, nr_plane, temp, 
                            cmax_disc_term, cdisp_step1, cdisc_single_jump);
        message_per_pixel_0(data, d, d - cmsg_step1, r - 1, l + 1, disp, disp + cmsg_step1, nr_plane, temp,
                            cmax_disc_term, cdisp_step1, cdisc_single_jump);
        message_per_pixel_0(data, l, u + cmsg_step1, d - cmsg_step1, l + 1, disp, disp - 1, nr_plane, temp,
                            cmax_disc_term, cdisp_step1, cdisc_single_jump);
        message_per_pixel_0(data, r, u + cmsg_step1, d - cmsg_step1, r - 1, disp, disp + 1, nr_plane, temp,
                            cmax_disc_term, cdisp_step1, cdisc_single_jump);
    }
}
__kernel void compute_message_1(__global float *u_, __global float *d_, __global float *l_, __global float *r_, 
                                __global const float *data_cost_selected, __global const float *selected_disp_pyr_cur, 
                                __global float *ctemp, int h, int w, int nr_plane, int i, 
                                float cmax_disc_term, int cdisp_step1, int cmsg_step1, float cdisc_single_jump)
{
    int y = get_global_id(1);
    int x = ((get_global_id(0)) << 1) + ((y + i) & 1);
   
    if (y > 0 && y < h - 1 && x > 0 && x < w - 1)
    {
        __global const float *data = data_cost_selected + y * cmsg_step1 + x;
      
        __global float *u = u_ + y * cmsg_step1 + x;
        __global float *d = d_ + y * cmsg_step1 + x;
        __global float *l = l_ + y * cmsg_step1 + x;
        __global float *r = r_ + y * cmsg_step1 + x;
       
        __global const float *disp = selected_disp_pyr_cur + y * cmsg_step1 + x;
        __global float *temp = ctemp + y * cmsg_step1 + x;
            
        message_per_pixel_1(data, u, r - 1, u + cmsg_step1, l + 1, disp, disp - cmsg_step1, nr_plane, temp, 
                            cmax_disc_term, cdisp_step1, cdisc_single_jump);
        message_per_pixel_1(data, d, d - cmsg_step1, r - 1, l + 1, disp, disp + cmsg_step1, nr_plane, temp,
                            cmax_disc_term, cdisp_step1, cdisc_single_jump);
        message_per_pixel_1(data, l, u + cmsg_step1, d - cmsg_step1, l + 1, disp, disp - 1, nr_plane, temp,
                            cmax_disc_term, cdisp_step1, cdisc_single_jump);
        message_per_pixel_1(data, r, u + cmsg_step1, d - cmsg_step1, r - 1, disp, disp + 1, nr_plane, temp,
                            cmax_disc_term, cdisp_step1, cdisc_single_jump);
    }
}

///////////////////////////////////////////////////////////////
/////////////////////////// output ////////////////////////////
///////////////////////////////////////////////////////////////
__kernel void compute_disp_0(__global const short *u_, __global const short *d_, __global const short *l_, 
                             __global const short *r_, __global const short * data_cost_selected, 
                             __global const short *disp_selected_pyr,
                             __global short* disp, 
                             int res_step, int cols, int rows, int nr_plane,
                             int cmsg_step1, int cdisp_step1)
{
    int x = get_global_id(0);
    int y = get_global_id(1);
   
    if (y > 0 && y < rows - 1 && x > 0 && x < cols - 1)
    {
        __global const short *data = data_cost_selected + y * cmsg_step1 + x;
        __global const short *disp_selected = disp_selected_pyr + y * cmsg_step1 + x;

        __global const short *u = u_ + (y+1) * cmsg_step1 + (x+0);
        __global const short *d = d_ + (y-1) * cmsg_step1 + (x+0);
        __global const short *l = l_ + (y+0) * cmsg_step1 + (x+1);
        __global const short *r = r_ + (y+0) * cmsg_step1 + (x-1);
       
        short best = 0;
        short best_val = SHRT_MAX;
       
        for (int i = 0; i < nr_plane; ++i)
        {
            int idx = i * cdisp_step1;
            short val = data[idx]+ u[idx] + d[idx] + l[idx] + r[idx];
                
            if (val < best_val)
            {
                best_val = val;
                best = disp_selected[idx];
            }
        }
        disp[res_step * y + x] = best;
    }
}
__kernel void compute_disp_1(__global const float *u_, __global const float *d_, __global const float *l_, 
                             __global const float *r_, __global const float *data_cost_selected, 
                             __global const float *disp_selected_pyr,
                             __global short *disp, 
                             int res_step, int cols, int rows, int nr_plane,
                             int cmsg_step1, int cdisp_step1)
{
    int x = get_global_id(0);
    int y = get_global_id(1);
   
    if (y > 0 && y < rows - 1 && x > 0 && x < cols - 1)
    {
        __global const float *data = data_cost_selected + y * cmsg_step1 + x;
        __global const float *disp_selected = disp_selected_pyr + y * cmsg_step1 + x;

        __global const float *u = u_ + (y+1) * cmsg_step1 + (x+0);
        __global const float *d = d_ + (y-1) * cmsg_step1 + (x+0);
        __global const float *l = l_ + (y+0) * cmsg_step1 + (x+1);
        __global const float *r = r_ + (y+0) * cmsg_step1 + (x-1);
       
        short best = 0;
        short best_val = SHRT_MAX;
        for (int i = 0; i < nr_plane; ++i)
        {
            int idx = i * cdisp_step1;
            float val = data[idx]+ u[idx] + d[idx] + l[idx] + r[idx];
                
            if (val < best_val)
            {
                best_val = val;
                best = convert_short_sat_rte(disp_selected[idx]);
            }
        }
        disp[res_step * y + x] = best;
    }
}