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