1 #ifndef boxm2_cone_update_image_functor_h
2 #define boxm2_cone_update_image_functor_h
3 //:
4 // \file
5 #include <vector>
6 #include <limits>
7 #include <iostream>
8 #include <cmath>
9 #include <boxm2/cpp/algo/boxm2_cast_ray_function.h>
10 #include <boxm2/cpp/algo/boxm2_mog3_grey_processor.h>
11 #include <vil/vil_image_view.h>
12 #ifdef _MSC_VER
13 #  include <vcl_msvc_warnings.h>
14 #endif
15 
16 class boxm2_cone_update_pass0_functor
17 {
18  public:
19   //: "default" constructor
20   boxm2_cone_update_pass0_functor() = default;
21 
init_data(std::vector<boxm2_data_base * > & datas,vil_image_view<float> * pre_img,vil_image_view<float> * vis_img,vil_image_view<float> * input_img)22   bool init_data(std::vector<boxm2_data_base*> & datas,
23                  vil_image_view<float> * pre_img,
24                  vil_image_view<float> * vis_img,
25                  vil_image_view<float> * input_img)
26   {
27     aux_data_=new boxm2_data<BOXM2_AUX>(datas[0]->data_buffer(),datas[0]->buffer_length(),datas[0]->block_id());
28     alpha_data_=new boxm2_data<BOXM2_GAMMA>(datas[1]->data_buffer(),datas[1]->buffer_length(),datas[1]->block_id());
29     mog3_data_=new boxm2_data<BOXM2_MOG3_GREY>(datas[2]->data_buffer(),datas[2]->buffer_length(),datas[2]->block_id());
30     pre_img_=pre_img;
31     vis_img_=vis_img;
32     PI_cum=0.0;
33     vol_cum=0.0;
34     vis_cum=1.0;
35 
36     input_img_=input_img;
37     return true;
38   }
39 
step_cell(float ray_vol,int index,unsigned i,unsigned j)40   inline bool step_cell(float ray_vol,int index,unsigned i,unsigned j)
41   {
42     boxm2_data<BOXM2_AUX>::datatype & aux=aux_data_->data()[index];
43     aux[0]+=ray_vol;
44     aux[1]+=ray_vol*(*input_img_)(i,j);
45     float PI=boxm2_processor_type<BOXM2_MOG3_GREY>::type::prob_density(mog3_data_->data()[index],(*input_img_)(i,j));
46     boxm2_data<BOXM2_GAMMA>::datatype gamma=alpha_data_->data()[index];
47     float temp=std::exp(-ray_vol*gamma);
48     PI_cum+=PI*ray_vol;
49     vol_cum+=ray_vol;
50     vis_cum*=temp;
51     return true;
52   }
53 
compute_ball_properties(unsigned i,unsigned j)54   inline bool compute_ball_properties(unsigned i,unsigned j)
55   {
56           float vis=(*vis_img_)(i,j);
57     float pre=(*pre_img_)(i,j);
58 
59     float PI=0.0;
60     if (vol_cum>1e-12f) PI=PI_cum/vol_cum;
61 
62     pre+=vis*(1-vis_cum)*PI;
63     vis*=vis_cum;
64 
65     (*pre_img_)(i,j)=pre;
66     (*vis_img_)(i,j)=vis;
67 
68     vis_cum=1.0;
69     PI_cum=0.0f;
70     vol_cum=0.0f;
71     return true;
72   }
73 
redistribute(float vol,int index)74   inline bool redistribute(float vol, int index){return true;}
75 
76  private:
77   boxm2_data<BOXM2_AUX> * aux_data_;
78   vil_image_view<float> * input_img_;
79   boxm2_data<BOXM2_GAMMA> * alpha_data_;
80   boxm2_data<BOXM2_MOG3_GREY> * mog3_data_;
81   vil_image_view<float> * pre_img_;
82   vil_image_view<float> * vis_img_;
83   float PI_cum;
84   float vol_cum;
85   float vis_cum;
86 };
87 
88 class boxm2_cone_update_pass1_functor
89 {
90  public:
91   //: "default" constructor
92   boxm2_cone_update_pass1_functor() = default;
93 
init_data(std::vector<boxm2_data_base * > & datas,vil_image_view<float> * pre_img,vil_image_view<float> * vis_img,vil_image_view<float> * input_img)94   bool init_data(std::vector<boxm2_data_base*> & datas,
95                  vil_image_view<float> * pre_img,
96                  vil_image_view<float> * vis_img,
97                  vil_image_view<float> * input_img)
98   {
99     aux_data_=new boxm2_data<BOXM2_AUX>(datas[0]->data_buffer(),datas[0]->buffer_length(),datas[0]->block_id());
100     alpha_data_=new boxm2_data<BOXM2_GAMMA>(datas[1]->data_buffer(),datas[1]->buffer_length(),datas[1]->block_id());
101     mog3_data_=new boxm2_data<BOXM2_MOG3_GREY>(datas[2]->data_buffer(),datas[2]->buffer_length(),datas[2]->block_id());
102     pre_img_=pre_img;
103     vis_img_=vis_img;
104     input_img_=input_img;
105     PI_cum=0.0;
106     vol_cum=0.0;
107     vis_cum=1.0;
108     return true;
109   }
110 
step_cell(float ray_vol,int index,unsigned i,unsigned j)111   inline bool step_cell(float ray_vol,int index,unsigned i,unsigned j)
112   {
113     boxm2_data<BOXM2_AUX>::datatype & aux=aux_data_->data()[index];
114     aux[0]+=ray_vol;
115     aux[1]+=ray_vol*(*input_img_)(i,j);
116 
117     float PI=boxm2_processor_type<BOXM2_MOG3_GREY>::type::prob_density(mog3_data_->data()[index],(*input_img_)(i,j));
118     boxm2_data<BOXM2_GAMMA>::datatype gamma=alpha_data_->data()[index];
119     float temp=std::exp(-ray_vol*gamma);
120     PI_cum+=PI*ray_vol;
121     vol_cum+=ray_vol;
122     vis_cum*=temp;
123 
124     return true;
125   }
126 
compute_ball_properties(unsigned i,unsigned j)127   inline bool compute_ball_properties(unsigned i,unsigned j)
128   {
129     float vis=(*vis_img_)(i,j);
130     float pre=(*pre_img_)(i,j);
131 
132     float PI=0.0;
133     if (vol_cum>1e-12f) PI=PI_cum/vol_cum;
134 
135     pre+=vis*(1-vis_cum)*PI;
136     vis*=vis_cum;
137 
138     (*pre_img_)(i,j)=pre;
139     (*vis_img_)(i,j)=vis;
140 
141     vis_cum=1.0;
142     PI_cum=0.0f;
143     vol_cum=0.0f;
144     return true;
145   }
146 
redistribute(float vol,int index)147   inline bool redistribute(float vol, int index) {return true;}
148 
149  private:
150   float PI_cum;
151   float vol_cum;
152   float vis_cum;
153   float pre_cum;
154   boxm2_data<BOXM2_AUX> * aux_data_;
155   boxm2_data<BOXM2_GAMMA> * alpha_data_;
156   boxm2_data<BOXM2_MOG3_GREY> * mog3_data_;
157   vil_image_view<float> * pre_img_;
158   vil_image_view<float> * vis_img_;
159   vil_image_view<float> * input_img_;
160 };
161 
162 
163 class boxm2_cone_update_pass2_functor
164 {
165  public:
166   //: "default" constructor
167   boxm2_cone_update_pass2_functor() = default;
168 
init_data(std::vector<boxm2_data_base * > & datas,vil_image_view<float> * pre_img,vil_image_view<float> * vis_img,vil_image_view<float> * norm_img,vil_image_view<float> * input_img)169   bool init_data(std::vector<boxm2_data_base*> & datas,
170                  vil_image_view<float> * pre_img,
171                  vil_image_view<float> * vis_img,
172                  vil_image_view<float> * norm_img,
173                  vil_image_view<float> * input_img)
174   {
175     aux_data_=new boxm2_data<BOXM2_AUX>(datas[0]->data_buffer(),datas[0]->buffer_length(),datas[0]->block_id());
176     alpha_data_=new boxm2_data<BOXM2_GAMMA>(datas[1]->data_buffer(),datas[1]->buffer_length(),datas[1]->block_id());
177     mog3_data_=new boxm2_data<BOXM2_MOG3_GREY>(datas[2]->data_buffer(),datas[2]->buffer_length(),datas[2]->block_id());
178 
179     pre_img_=pre_img;
180     vis_img_=vis_img;
181     norm_img_=norm_img;
182     input_img_=input_img;
183 
184     vis_cum=1.0;
185     PI_cum=0.0;
186     vol_cum=0.0;
187     beta_=1;
188 
189     return true;
190   }
191 
step_cell(float ray_vol,int index,unsigned i,unsigned j)192   inline bool step_cell(float ray_vol,int index,unsigned i,unsigned j)
193   {
194     boxm2_data<BOXM2_AUX>::datatype & aux=aux_data_->data()[index];
195     //if (aux[0]<1e-10f) return true;
196     float PI=boxm2_processor_type<BOXM2_MOG3_GREY>::type::prob_density(mog3_data_->data()[index], (*input_img_)(i,j));
197     float vis=(*vis_img_)(i,j);
198     boxm2_data<BOXM2_GAMMA>::datatype gamma=alpha_data_->data()[index];
199     aux[3]+=vis*ray_vol;
200     float temp=std::exp(-ray_vol*gamma);
201     PI_cum+=PI*ray_vol;
202     vol_cum+=ray_vol;
203     vis_cum*=temp;
204     return true;
205   }
206 
compute_ball_properties(unsigned i,unsigned j)207   inline bool compute_ball_properties(unsigned i,unsigned j)
208   {
209     float vis=(*vis_img_)(i,j);
210     float pre=(*pre_img_)(i,j);
211 
212     float PI=0.0;
213     if (vol_cum>1e-12f) PI=PI_cum/vol_cum;
214 
215     beta_ = (pre+vis*PI)/(*norm_img_)(i,j);
216     pre+=vis*(1-vis_cum)*PI;
217     vis*=vis_cum;
218 
219     (*pre_img_)(i,j)=pre;
220     (*vis_img_)(i,j)=vis;
221 
222     vis_cum=1.0;
223     PI_cum=0.0f;
224     vol_cum=0.0f;
225 
226     return true;
227   }
228 
redistribute(float vol,int index)229   inline bool redistribute(float vol, int index)
230   {
231       boxm2_data<BOXM2_AUX>::datatype & aux=aux_data_->data()[index];
232       aux[2]+=beta_*vol;
233       return true;
234   }
235 
236  private:
237   float vis_cum;
238   float pre_cum;
239 
240   float beta_;
241   float PI_cum;
242   float vol_cum;
243   boxm2_data<BOXM2_AUX> * aux_data_;
244   boxm2_data<BOXM2_GAMMA> * alpha_data_;
245   boxm2_data<BOXM2_MOG3_GREY> * mog3_data_;
246   vil_image_view<float> * input_img_;
247   vil_image_view<float> * pre_img_;
248   vil_image_view<float> * vis_img_;
249   vil_image_view<float> * norm_img_;
250 };
251 
252 class boxm2_cone_update_data_functor
253 {
254  public:
255   //: "default" constructor
256   boxm2_cone_update_data_functor() = default;
257 
init_data(std::vector<boxm2_data_base * > & datas,float block_len,int max_levels)258   bool init_data(std::vector<boxm2_data_base*> & datas, float block_len, int max_levels)
259   {
260     aux_data_=new boxm2_data<BOXM2_AUX>(datas[0]->data_buffer(),datas[0]->buffer_length(),datas[0]->block_id());
261     alpha_data_=new boxm2_data<BOXM2_GAMMA>(datas[1]->data_buffer(),datas[1]->buffer_length(),datas[1]->block_id());
262     mog3_data_=new boxm2_data<BOXM2_MOG3_GREY>(datas[2]->data_buffer(),datas[2]->buffer_length(),datas[2]->block_id());
263     nobs_data_=new boxm2_data<BOXM2_NUM_OBS>(datas[3]->data_buffer(),datas[3]->buffer_length(),datas[3]->block_id());
264     alpha_min_ = -std::log(1.f-0.0001f)/float(std::pow(block_len/max_levels,3));
265     return true;
266   }
267 
process_cell(int index)268   inline bool process_cell(int index)
269   {
270     boxm2_data<BOXM2_AUX>::datatype & aux=aux_data_->data()[index];
271 
272     if (aux[0]>1e-10f)
273     {
274       float beta    =aux[2]/aux[0];
275       float vis     =aux[3]/aux[0];
276       float mean_obs=aux[1]/aux[0];
277 
278       boxm2_data<BOXM2_GAMMA>::datatype     & alpha=alpha_data_->data()[index];
279       boxm2_data<BOXM2_MOG3_GREY>::datatype & mog3 =mog3_data_->data()[index];
280       boxm2_data<BOXM2_NUM_OBS>::datatype   & nobs =nobs_data_->data()[index];
281 
282       alpha=alpha*beta;
283 
284       vnl_vector_fixed<float,4> nobs_float;
285       nobs_float[0]=(float)nobs[0];
286       nobs_float[1]=(float)nobs[1];
287       nobs_float[2]=(float)nobs[2];
288       //: converting flot to short
289       nobs_float[3]=((float)nobs[3])/100.0f;
290       boxm2_processor_type<BOXM2_MOG3_GREY>::type::update_gauss_mixture_3(mog3,nobs_float, mean_obs,vis,0.09f, 0.03f);
291 
292       nobs[0]=(unsigned short)nobs_float[0];
293       nobs[1]=(unsigned short)nobs_float[1];
294       nobs[2]=(unsigned short)nobs_float[2];
295       nobs[3]=(unsigned short)(nobs_float[3]*100.0f);
296 
297       aux[0]=0.0;
298       aux[1]=0.0;
299       aux[2]=0.0;
300       aux[3]=0.0;
301     }
302     return true;
303   }
304 
305  private:
306   boxm2_data<BOXM2_AUX>       * aux_data_;
307   boxm2_data<BOXM2_GAMMA>     * alpha_data_;
308   boxm2_data<BOXM2_MOG3_GREY> * mog3_data_;
309   boxm2_data<BOXM2_NUM_OBS>   * nobs_data_;
310   float alpha_min_;
311 };
312 
313 #endif
314