CLAHE Python bindings
[profile/ivi/opencv.git] / modules / ocl / src / stereobp.cpp
1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 //  By downloading, copying, installing or using the software you agree to this license.
6 //  If you do not agree to this license, do not download, install,
7 //  copy or use the software.
8 //
9 //
10 //                           License Agreement
11 //                For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2010-2012, Multicoreware, Inc., all rights reserved.
14 // Copyright (C) 2010-2012, Institute Of Software Chinese Academy Of Science, all rights reserved.
15 // Copyright (C) 2010-2012, Advanced Micro Devices, Inc., all rights reserved.
16 // Third party copyrights are property of their respective owners.
17 //
18 // @Authors
19 //    Jia Haipeng, jiahaipeng95@gmail.com
20 //    Peng Xiao,   pengxiao@outlook.com
21 // Redistribution and use in source and binary forms, with or without modification,
22 // are permitted provided that the following conditions are met:
23 //
24 //   * Redistribution's of source code must retain the above copyright notice,
25 //     this list of conditions and the following disclaimer.
26 //
27 //   * Redistribution's in binary form must reproduce the above copyright notice,
28 //     this list of conditions and the following disclaimer in the documentation
29 //     and/or other oclMaterials provided with the distribution.
30 //
31 //   * The name of the copyright holders may not be used to endorse or promote products
32 //     derived from this software without specific prior written permission.
33 //
34 // This software is provided by the copyright holders and contributors "as is" and
35 // any express or implied warranties, including, but not limited to, the implied
36 // warranties of merchantability and fitness for a particular purpose are disclaimed.
37 // In no event shall the Intel Corporation or contributors be liable for any direct,
38 // indirect, incidental, special, exemplary, or consequential damages
39 // (including, but not limited to, procurement of substitute goods or services;
40 // loss of use, data, or profits; or business interruption) however caused
41 // and on any theory of liability, whether in contract, strict liability,
42 // or tort (including negligence or otherwise) arising in any way out of
43 // the use of this software, even if advised of the possibility of such damage.
44 //
45 //M*/
46
47 #include "precomp.hpp"
48 #include <vector>
49 #include <cstdio>
50
51 using namespace cv;
52 using namespace cv::ocl;
53 using namespace std;
54
55 ////////////////////////////////////////////////////////////////////////
56 ///////////////// stereoBP /////////////////////////////////////////////
57 ////////////////////////////////////////////////////////////////////////
58
59 namespace cv
60 {
61     namespace ocl
62     {
63
64         ///////////////////////////OpenCL kernel strings///////////////////////////
65         extern const char *stereobp;
66     }
67
68 }
69 namespace cv
70 {
71     namespace ocl
72     {
73         namespace stereoBP
74         {
75             //////////////////////////////////////////////////////////////////////////
76             //////////////////////////////common////////////////////////////////////
77             ////////////////////////////////////////////////////////////////////////
78             typedef struct
79             {
80                 int   cndisp;
81                 float cmax_data_term;
82                 float cdata_weight;
83                 float cmax_disc_term;
84                 float cdisc_single_jump;
85             } con_struct_t;
86
87             cl_mem cl_con_struct =  NULL;
88             static void load_constants(int ndisp, float max_data_term, float data_weight,
89                                 float max_disc_term, float disc_single_jump)
90             {
91                 con_struct_t *con_struct = new con_struct_t;
92                 con_struct -> cndisp            = ndisp;
93                 con_struct -> cmax_data_term    = max_data_term;
94                 con_struct -> cdata_weight      = data_weight;
95                 con_struct -> cmax_disc_term    = max_disc_term;
96                 con_struct -> cdisc_single_jump = disc_single_jump;
97
98                 cl_con_struct = load_constant(*((cl_context*)getoclContext()), *((cl_command_queue*)getoclCommandQueue()), (void *)con_struct,
99                                               sizeof(con_struct_t));
100
101                 delete con_struct;
102             }
103             static void release_constants()
104             {
105                 openCLFree(cl_con_struct);
106             }
107             static inline int divUp(int total, int grain)
108             {
109                 return (total + grain - 1) / grain;
110             }
111             /////////////////////////////////////////////////////////////////////////////
112             ///////////////////////////comp data////////////////////////////////////////
113             /////////////////////////////////////////////////////////////////////////
114             static void  comp_data_call(const oclMat &left, const oclMat &right, oclMat &data, int /*disp*/,
115                                 float /*cmax_data_term*/, float /*cdata_weight*/)
116             {
117                 Context  *clCxt = left.clCxt;
118                 int channels = left.oclchannels();
119                 int data_type = data.type();
120
121                 string kernelName = "comp_data";
122
123                 vector<pair<size_t , const void *> > args;
124
125                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&left.data));
126                 args.push_back( make_pair( sizeof(cl_int) , (void *)&left.rows));
127                 args.push_back( make_pair( sizeof(cl_int) , (void *)&left.cols));
128                 args.push_back( make_pair( sizeof(cl_int) , (void *)&left.step));
129                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&right.data));
130                 args.push_back( make_pair( sizeof(cl_int) , (void *)&right.step));
131                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&data.data));
132                 args.push_back( make_pair( sizeof(cl_int) , (void *)&data.step));
133                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&cl_con_struct));
134
135                 size_t gt[3] = {left.cols, left.rows, 1}, lt[3] = {16, 16, 1};
136
137                 const int OPT_SIZE = 50;
138                 char cn_opt [OPT_SIZE] = "";
139                 sprintf( cn_opt, "%s -D CN=%d", 
140                     (data_type == CV_16S ? "-D T_SHORT":"-D T_FLOAT"),
141                     channels
142                     );
143                 openCLExecuteKernel(clCxt, &stereobp, kernelName, gt, lt, args, -1, -1, cn_opt);
144             }
145             ///////////////////////////////////////////////////////////////////////////////////
146             /////////////////////////data set down////////////////////////////////////////////
147             /////////////////////////////////////////////////////////////////////////////////
148             static void data_step_down_call(int dst_cols, int dst_rows, int src_rows,
149                 const oclMat &src, oclMat &dst, int disp)
150             {
151                 Context  *clCxt = src.clCxt;
152                 int data_type = src.type();
153
154                 string kernelName = "data_step_down";
155
156                 vector<pair<size_t , const void *> > args;
157
158                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&src.data));
159                 args.push_back( make_pair( sizeof(cl_int) , (void *)&src_rows));
160                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&dst.data));
161                 args.push_back( make_pair( sizeof(cl_int) , (void *)&dst_rows));
162                 args.push_back( make_pair( sizeof(cl_int) , (void *)&dst_cols));
163                 args.push_back( make_pair( sizeof(cl_int) , (void *)&src.step));
164                 args.push_back( make_pair( sizeof(cl_int) , (void *)&dst.step));
165                 args.push_back( make_pair( sizeof(cl_int) , (void *)&disp));
166
167                 size_t gt[3] = {dst_cols, dst_rows, 1}, lt[3] = {16, 16, 1};
168                 const char* t_opt  = data_type == CV_16S ? "-D T_SHORT":"-D T_FLOAT";
169                 openCLExecuteKernel(clCxt, &stereobp, kernelName, gt, lt, args, -1, -1, t_opt);
170             }
171             /////////////////////////////////////////////////////////////////////////////////
172             ///////////////////////////live up message////////////////////////////////////////
173             /////////////////////////////////////////////////////////////////////////////////
174             static void level_up_message_call(int dst_cols, int dst_rows, int src_rows,
175                 oclMat &src, oclMat &dst, int ndisp)
176             {
177                 Context  *clCxt = src.clCxt;
178                 int data_type = src.type();
179
180                 string kernelName = "level_up_message";
181                 vector<pair<size_t , const void *> > args;
182
183                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&src.data));
184                 args.push_back( make_pair( sizeof(cl_int) , (void *)&src_rows));
185                 args.push_back( make_pair( sizeof(cl_int) , (void *)&src.step));
186                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&dst.data));
187                 args.push_back( make_pair( sizeof(cl_int) , (void *)&dst_rows));
188                 args.push_back( make_pair( sizeof(cl_int) , (void *)&dst_cols));
189                 args.push_back( make_pair( sizeof(cl_int) , (void *)&dst.step));
190                 args.push_back( make_pair( sizeof(cl_int) , (void *)&ndisp));
191
192                 size_t gt[3] = {dst_cols, dst_rows, 1}, lt[3] = {16, 16, 1};
193                 const char* t_opt  = data_type == CV_16S ? "-D T_SHORT":"-D T_FLOAT";
194                 openCLExecuteKernel(clCxt, &stereobp, kernelName, gt, lt, args, -1, -1, t_opt);
195             }
196             static void level_up_messages_calls(int dst_idx, int dst_cols, int dst_rows, int src_rows,
197                                          oclMat *mus, oclMat *mds, oclMat *mls, oclMat *mrs,
198                                          int ndisp)
199             {
200                 int src_idx = (dst_idx + 1) & 1;
201
202                 level_up_message_call(dst_cols, dst_rows, src_rows,
203                                       mus[src_idx], mus[dst_idx], ndisp);
204
205                 level_up_message_call(dst_cols, dst_rows, src_rows,
206                                       mds[src_idx], mds[dst_idx], ndisp);
207
208                 level_up_message_call(dst_cols, dst_rows, src_rows,
209                                       mls[src_idx], mls[dst_idx], ndisp);
210
211                 level_up_message_call(dst_cols, dst_rows, src_rows,
212                                       mrs[src_idx], mrs[dst_idx], ndisp);
213             }
214             //////////////////////////////////////////////////////////////////////////////////
215             //////////////////////////////cals_all_iterations_call///////////////////////////
216             /////////////////////////////////////////////////////////////////////////////////
217             static void calc_all_iterations_call(int cols, int rows, oclMat &u, oclMat &d,
218                 oclMat &l, oclMat &r, oclMat &data,
219                 int t, int cndisp, float cmax_disc_term,
220                 float cdisc_single_jump)
221             {
222                 Context  *clCxt = l.clCxt;
223                 int data_type = u.type();
224
225                 string kernelName = "one_iteration";
226
227                 vector<pair<size_t , const void *> > args;
228
229                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&u.data));
230                 args.push_back( make_pair( sizeof(cl_int) , (void *)&u.step));
231                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&data.data));
232                 args.push_back( make_pair( sizeof(cl_int) , (void *)&data.step));
233                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&d.data));
234                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&l.data));
235                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&r.data));
236                 args.push_back( make_pair( sizeof(cl_int) , (void *)&t));
237                 args.push_back( make_pair( sizeof(cl_int) , (void *)&cols));
238                 args.push_back( make_pair( sizeof(cl_int) , (void *)&rows));
239                 args.push_back( make_pair( sizeof(cl_int) , (void *)&cndisp));
240                 args.push_back( make_pair( sizeof(cl_float) , (void *)&cmax_disc_term));
241                 args.push_back( make_pair( sizeof(cl_float) , (void *)&cdisc_single_jump));
242
243                 size_t gt[3] = {cols, rows, 1}, lt[3] = {16, 16, 1};
244                 const char* t_opt  = data_type == CV_16S ? "-D T_SHORT":"-D T_FLOAT";
245                 openCLExecuteKernel(clCxt, &stereobp, kernelName, gt, lt, args, -1, -1, t_opt);
246             }
247
248             static void calc_all_iterations_calls(int cols, int rows, int iters, oclMat &u,
249                                            oclMat &d, oclMat &l, oclMat &r,
250                                            oclMat &data, int cndisp, float cmax_disc_term,
251                                            float cdisc_single_jump)
252             {
253                 for(int t = 0; t < iters; ++t)
254                     calc_all_iterations_call(cols, rows, u, d, l, r, data, t, cndisp,
255                                              cmax_disc_term, cdisc_single_jump);
256             }
257             ///////////////////////////////////////////////////////////////////////////////
258             ///////////////////////output///////////////////////////////////////////////////
259             ////////////////////////////////////////////////////////////////////////////////
260             static void output_call(const oclMat &u, const oclMat &d, const oclMat l, const oclMat &r,
261                 const oclMat &data, oclMat &disp, int ndisp)
262             {
263                 Context  *clCxt = u.clCxt;
264                 int data_type = u.type();
265
266                 string kernelName = "output";
267
268                 vector<pair<size_t , const void *> > args;
269
270                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&u.data));
271                 args.push_back( make_pair( sizeof(cl_int) , (void *)&u.step));
272                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&d.data));
273                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&l.data));
274                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&r.data));
275                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&data.data));
276                 args.push_back( make_pair( sizeof(cl_mem) , (void *)&disp.data));
277                 args.push_back( make_pair( sizeof(cl_int) , (void *)&disp.rows));
278                 args.push_back( make_pair( sizeof(cl_int) , (void *)&disp.cols));
279                 args.push_back( make_pair( sizeof(cl_int) , (void *)&disp.step));
280                 args.push_back( make_pair( sizeof(cl_int) , (void *)&ndisp));
281
282                 size_t gt[3] = {disp.cols, disp.rows, 1}, lt[3] = {16, 16, 1};
283                 const char* t_opt  = data_type == CV_16S ? "-D T_SHORT":"-D T_FLOAT";
284                 openCLExecuteKernel(clCxt, &stereobp, kernelName, gt, lt, args, -1, -1, t_opt);
285             }
286         }
287     }
288 }
289 namespace
290 {
291     const float DEFAULT_MAX_DATA_TERM = 10.0f;
292     const float DEFAULT_DATA_WEIGHT = 0.07f;
293     const float DEFAULT_MAX_DISC_TERM = 1.7f;
294     const float DEFAULT_DISC_SINGLE_JUMP = 1.0f;
295 }
296
297 void cv::ocl::StereoBeliefPropagation::estimateRecommendedParams(int width, int height, int &ndisp, int &iters, int &levels)
298 {
299     ndisp = width / 4;
300     if ((ndisp & 1) != 0)
301         ndisp++;
302
303     int mm = ::max(width, height);
304     iters = mm / 100 + 2;
305
306     levels = (int)(::log(static_cast<double>(mm)) + 1) * 4 / 5;
307     if (levels == 0) levels++;
308 }
309
310 cv::ocl::StereoBeliefPropagation::StereoBeliefPropagation(int ndisp_, int iters_, int levels_, int msg_type_)
311     : ndisp(ndisp_), iters(iters_), levels(levels_),
312       max_data_term(DEFAULT_MAX_DATA_TERM), data_weight(DEFAULT_DATA_WEIGHT),
313       max_disc_term(DEFAULT_MAX_DISC_TERM), disc_single_jump(DEFAULT_DISC_SINGLE_JUMP),
314       msg_type(msg_type_), datas(levels_)
315 {
316 }
317
318 cv::ocl::StereoBeliefPropagation::StereoBeliefPropagation(int ndisp_, int iters_, int levels_, float max_data_term_, float data_weight_, float max_disc_term_, float disc_single_jump_, int msg_type_)
319     : ndisp(ndisp_), iters(iters_), levels(levels_),
320       max_data_term(max_data_term_), data_weight(data_weight_),
321       max_disc_term(max_disc_term_), disc_single_jump(disc_single_jump_),
322       msg_type(msg_type_), datas(levels_)
323 {
324 }
325
326 namespace
327 {
328     class StereoBeliefPropagationImpl
329     {
330     public:
331         StereoBeliefPropagationImpl(StereoBeliefPropagation &rthis_,
332                                     oclMat &u_, oclMat &d_, oclMat &l_, oclMat &r_,
333                                     oclMat &u2_, oclMat &d2_, oclMat &l2_, oclMat &r2_,
334                                     vector<oclMat> &datas_, oclMat &out_)
335             : rthis(rthis_), u(u_), d(d_), l(l_), r(r_), u2(u2_), d2(d2_), l2(l2_), r2(r2_), datas(datas_), out(out_),
336               zero(Scalar::all(0)), scale(rthis_.msg_type == CV_32F ? 1.0f : 10.0f)
337         {
338             CV_Assert(0 < rthis.ndisp && 0 < rthis.iters && 0 < rthis.levels);
339             CV_Assert(rthis.msg_type == CV_32F || rthis.msg_type == CV_16S);
340             CV_Assert(rthis.msg_type == CV_32F || (1 << (rthis.levels - 1)) * scale * rthis.max_data_term < numeric_limits<short>::max());
341         }
342
343         void operator()(const oclMat &left, const oclMat &right, oclMat &disp)
344         {
345             CV_Assert(left.size() == right.size() && left.type() == right.type());
346             CV_Assert(left.type() == CV_8UC1 || left.type() == CV_8UC3 || left.type() == CV_8UC4);
347
348             rows = left.rows;
349             cols = left.cols;
350
351             int divisor = (int)pow(2.f, rthis.levels - 1.0f);
352             int lowest_cols = cols / divisor;
353             int lowest_rows = rows / divisor;
354             const int min_image_dim_size = 2;
355             CV_Assert(min(lowest_cols, lowest_rows) > min_image_dim_size);
356
357             init();
358
359             datas[0].create(rows * rthis.ndisp, cols, rthis.msg_type);
360             datas[0].setTo(Scalar_<short>::all(0));
361
362             cv::ocl::stereoBP::comp_data_call(left, right, datas[0], rthis.ndisp, rthis.max_data_term, scale * rthis.data_weight);
363             calcBP(disp);
364         }
365
366         void operator()(const oclMat &data, oclMat &disp)
367         {
368             CV_Assert((data.type() == rthis.msg_type) && (data.rows % rthis.ndisp == 0));
369
370             rows = data.rows / rthis.ndisp;
371             cols = data.cols;
372
373             int divisor = (int)pow(2.f, rthis.levels - 1.0f);
374             int lowest_cols = cols / divisor;
375             int lowest_rows = rows / divisor;
376             const int min_image_dim_size = 2;
377             CV_Assert(min(lowest_cols, lowest_rows) > min_image_dim_size);
378
379             init();
380
381             datas[0] = data;
382
383             calcBP(disp);
384         }
385     private:
386         void init()
387         {
388             u.create(rows * rthis.ndisp, cols, rthis.msg_type);
389             d.create(rows * rthis.ndisp, cols, rthis.msg_type);
390             l.create(rows * rthis.ndisp, cols, rthis.msg_type);
391             r.create(rows * rthis.ndisp, cols, rthis.msg_type);
392
393             if (rthis.levels & 1)
394             {
395                 //can clear less area
396                 u = zero;
397                 d = zero;
398                 l = zero;
399                 r = zero;
400             }
401
402             if (rthis.levels > 1)
403             {
404                 int less_rows = (rows + 1) / 2;
405                 int less_cols = (cols + 1) / 2;
406
407                 u2.create(less_rows * rthis.ndisp, less_cols, rthis.msg_type);
408                 d2.create(less_rows * rthis.ndisp, less_cols, rthis.msg_type);
409                 l2.create(less_rows * rthis.ndisp, less_cols, rthis.msg_type);
410                 r2.create(less_rows * rthis.ndisp, less_cols, rthis.msg_type);
411
412                 if ((rthis.levels & 1) == 0)
413                 {
414                     u2 = zero;
415                     d2 = zero;
416                     l2 = zero;
417                     r2 = zero;
418                 }
419             }
420
421             cv::ocl::stereoBP::load_constants(rthis.ndisp, rthis.max_data_term, scale * rthis.data_weight,
422                                               scale * rthis.max_disc_term, scale * rthis.disc_single_jump);
423
424             datas.resize(rthis.levels);
425             cols_all.resize(rthis.levels);
426             rows_all.resize(rthis.levels);
427
428             cols_all[0] = cols;
429             rows_all[0] = rows;
430         }
431
432         void calcBP(oclMat &disp)
433         {
434             using namespace cv::ocl::stereoBP;
435
436             for (int i = 1; i < rthis.levels; ++i)
437             {
438                 cols_all[i] = (cols_all[i - 1] + 1) / 2;
439                 rows_all[i] = (rows_all[i - 1] + 1) / 2;
440
441                 datas[i].create(rows_all[i] * rthis.ndisp, cols_all[i], rthis.msg_type);
442                 datas[i].setTo(Scalar_<short>::all(0));
443
444                 data_step_down_call(cols_all[i], rows_all[i], rows_all[i - 1],
445                                     datas[i - 1], datas[i], rthis.ndisp);
446             }
447
448             oclMat mus[] = {u, u2};
449             oclMat mds[] = {d, d2};
450             oclMat mrs[] = {r, r2};
451             oclMat mls[] = {l, l2};
452
453             int mem_idx = (rthis.levels & 1) ? 0 : 1;
454
455             for (int i = rthis.levels - 1; i >= 0; --i)
456             {
457                 // for lower level we have already computed messages by setting to zero
458                 if (i != rthis.levels - 1)
459                     level_up_messages_calls(mem_idx, cols_all[i], rows_all[i], rows_all[i + 1],
460                                             mus, mds, mls, mrs, rthis.ndisp);
461
462                 calc_all_iterations_calls(cols_all[i], rows_all[i], rthis.iters, mus[mem_idx],
463                                           mds[mem_idx], mls[mem_idx], mrs[mem_idx], datas[i],
464                                           rthis.ndisp, scale * rthis.max_disc_term,
465                                           scale * rthis.disc_single_jump);
466
467                 mem_idx = (mem_idx + 1) & 1;
468             }
469             if (disp.empty())
470                 disp.create(rows, cols, CV_16S);
471
472             out = ((disp.type() == CV_16S) ? disp : (out.create(rows, cols, CV_16S), out));
473             out = zero;
474
475             output_call(u, d, l, r, datas.front(), out, rthis.ndisp);
476
477             if (disp.type() != CV_16S)
478                 out.convertTo(disp, disp.type());
479
480             release_constants();
481         }
482         StereoBeliefPropagationImpl& operator=(const StereoBeliefPropagationImpl&);
483
484         StereoBeliefPropagation &rthis;
485
486         oclMat &u;
487         oclMat &d;
488         oclMat &l;
489         oclMat &r;
490
491         oclMat &u2;
492         oclMat &d2;
493         oclMat &l2;
494         oclMat &r2;
495
496         vector<oclMat> &datas;
497         oclMat &out;
498
499         const Scalar zero;
500         const float scale;
501
502         int rows, cols;
503
504         vector<int> cols_all, rows_all;
505     };
506 }
507
508 void cv::ocl::StereoBeliefPropagation::operator()(const oclMat &left, const oclMat &right, oclMat &disp)
509 {
510     ::StereoBeliefPropagationImpl impl(*this, u, d, l, r, u2, d2, l2, r2, datas, out);
511     impl(left, right, disp);
512 }
513
514 void cv::ocl::StereoBeliefPropagation::operator()(const oclMat &data, oclMat &disp)
515 {
516     ::StereoBeliefPropagationImpl impl(*this, u, d, l, r, u2, d2, l2, r2, datas, out);
517     impl(data, disp);
518 }
519