1 /*
2  * Copyright (C) 2011 The Android Open Source Project
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  *      http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 // $Id: dbreg.cpp,v 1.31 2011/06/17 14:04:32 mbansal Exp $
18 #include "dbreg.h"
19 #include <string.h>
20 #include <stdio.h>
21 
22 
23 #if PROFILE
24 #endif
25 
26 //#include <iostream>
27 
db_FrameToReferenceRegistration()28 db_FrameToReferenceRegistration::db_FrameToReferenceRegistration() :
29   m_initialized(false),m_nr_matches(0),m_over_allocation(256),m_nr_bins(20),m_max_cost_pix(30), m_quarter_resolution(false)
30 {
31   m_reference_image = NULL;
32   m_aligned_ins_image = NULL;
33 
34   m_quarter_res_image = NULL;
35   m_horz_smooth_subsample_image = NULL;
36 
37   m_x_corners_ref = NULL;
38   m_y_corners_ref = NULL;
39 
40   m_x_corners_ins = NULL;
41   m_y_corners_ins = NULL;
42 
43   m_match_index_ref = NULL;
44   m_match_index_ins = NULL;
45 
46   m_inlier_indices = NULL;
47 
48   m_num_inlier_indices = 0;
49 
50   m_temp_double = NULL;
51   m_temp_int = NULL;
52 
53   m_corners_ref = NULL;
54   m_corners_ins = NULL;
55 
56   m_sq_cost = NULL;
57   m_cost_histogram = NULL;
58 
59   profile_string = NULL;
60 
61   db_Identity3x3(m_K);
62   db_Identity3x3(m_H_ref_to_ins);
63   db_Identity3x3(m_H_dref_to_ref);
64 
65   m_sq_cost_computed = false;
66   m_reference_set = false;
67 
68   m_reference_update_period = 0;
69   m_nr_frames_processed = 0;
70 
71   return;
72 }
73 
~db_FrameToReferenceRegistration()74 db_FrameToReferenceRegistration::~db_FrameToReferenceRegistration()
75 {
76   Clean();
77 }
78 
Clean()79 void db_FrameToReferenceRegistration::Clean()
80 {
81   if ( m_reference_image )
82     db_FreeImage_u(m_reference_image,m_im_height);
83 
84   if ( m_aligned_ins_image )
85     db_FreeImage_u(m_aligned_ins_image,m_im_height);
86 
87   if ( m_quarter_res_image )
88   {
89     db_FreeImage_u(m_quarter_res_image, m_im_height);
90   }
91 
92   if ( m_horz_smooth_subsample_image )
93   {
94     db_FreeImage_u(m_horz_smooth_subsample_image, m_im_height*2);
95   }
96 
97   delete [] m_x_corners_ref;
98   delete [] m_y_corners_ref;
99 
100   delete [] m_x_corners_ins;
101   delete [] m_y_corners_ins;
102 
103   delete [] m_match_index_ref;
104   delete [] m_match_index_ins;
105 
106   delete [] m_temp_double;
107   delete [] m_temp_int;
108 
109   delete [] m_corners_ref;
110   delete [] m_corners_ins;
111 
112   delete [] m_sq_cost;
113   delete [] m_cost_histogram;
114 
115   delete [] m_inlier_indices;
116 
117   if(profile_string)
118     delete [] profile_string;
119 
120   m_reference_image = NULL;
121   m_aligned_ins_image = NULL;
122 
123   m_quarter_res_image = NULL;
124   m_horz_smooth_subsample_image = NULL;
125 
126   m_x_corners_ref = NULL;
127   m_y_corners_ref = NULL;
128 
129   m_x_corners_ins = NULL;
130   m_y_corners_ins = NULL;
131 
132   m_match_index_ref = NULL;
133   m_match_index_ins = NULL;
134 
135   m_inlier_indices = NULL;
136 
137   m_temp_double = NULL;
138   m_temp_int = NULL;
139 
140   m_corners_ref = NULL;
141   m_corners_ins = NULL;
142 
143   m_sq_cost = NULL;
144   m_cost_histogram = NULL;
145 }
146 
Init(int width,int height,int homography_type,int max_iterations,bool linear_polish,bool quarter_resolution,double scale,unsigned int reference_update_period,bool do_motion_smoothing,double motion_smoothing_gain,int nr_samples,int chunk_size,int cd_target_nr_corners,double cm_max_disparity,bool cm_use_smaller_matching_window,int cd_nr_horz_blocks,int cd_nr_vert_blocks)147 void db_FrameToReferenceRegistration::Init(int width, int height,
148                        int    homography_type,
149                        int    max_iterations,
150                        bool   linear_polish,
151                        bool   quarter_resolution,
152                        double scale,
153                        unsigned int reference_update_period,
154                        bool   do_motion_smoothing,
155                        double motion_smoothing_gain,
156                        int    nr_samples,
157                        int    chunk_size,
158                        int    cd_target_nr_corners,
159                        double cm_max_disparity,
160                            bool   cm_use_smaller_matching_window,
161                        int    cd_nr_horz_blocks,
162                        int    cd_nr_vert_blocks
163                        )
164 {
165   Clean();
166 
167   m_reference_update_period = reference_update_period;
168   m_nr_frames_processed = 0;
169 
170   m_do_motion_smoothing = do_motion_smoothing;
171   m_motion_smoothing_gain = motion_smoothing_gain;
172 
173   m_stab_smoother.setSmoothingFactor(m_motion_smoothing_gain);
174 
175   m_quarter_resolution = quarter_resolution;
176 
177   profile_string = new char[10240];
178 
179   if (m_quarter_resolution == true)
180   {
181     width = width/2;
182     height = height/2;
183 
184     m_horz_smooth_subsample_image = db_AllocImage_u(width,height*2,m_over_allocation);
185     m_quarter_res_image = db_AllocImage_u(width,height,m_over_allocation);
186   }
187 
188   m_im_width = width;
189   m_im_height = height;
190 
191   double temp[9];
192   db_Approx3DCalMat(m_K,temp,m_im_width,m_im_height);
193 
194   m_homography_type = homography_type;
195   m_max_iterations = max_iterations;
196   m_scale = 2/(m_K[0]+m_K[4]);
197   m_nr_samples = nr_samples;
198   m_chunk_size = chunk_size;
199 
200   double outlier_t1 = 5.0;
201 
202   m_outlier_t2 = outlier_t1*outlier_t1;//*m_scale*m_scale;
203 
204   m_current_is_reference = false;
205 
206   m_linear_polish = linear_polish;
207 
208   m_reference_image = db_AllocImage_u(m_im_width,m_im_height,m_over_allocation);
209   m_aligned_ins_image = db_AllocImage_u(m_im_width,m_im_height,m_over_allocation);
210 
211   // initialize feature detection and matching:
212   //m_max_nr_corners = m_cd.Init(m_im_width,m_im_height,cd_target_nr_corners,cd_nr_horz_blocks,cd_nr_vert_blocks,0.0,0.0);
213   m_max_nr_corners = m_cd.Init(m_im_width,m_im_height,cd_target_nr_corners,cd_nr_horz_blocks,cd_nr_vert_blocks,DB_DEFAULT_ABS_CORNER_THRESHOLD/500.0,0.0);
214 
215     int use_21 = 0;
216   m_max_nr_matches = m_cm.Init(m_im_width,m_im_height,cm_max_disparity,m_max_nr_corners,DB_DEFAULT_NO_DISPARITY,cm_use_smaller_matching_window,use_21);
217 
218   // allocate space for corner feature locations for reference and inspection images:
219   m_x_corners_ref = new double [m_max_nr_corners];
220   m_y_corners_ref = new double [m_max_nr_corners];
221 
222   m_x_corners_ins = new double [m_max_nr_corners];
223   m_y_corners_ins = new double [m_max_nr_corners];
224 
225   // allocate space for match indices:
226   m_match_index_ref = new int [m_max_nr_matches];
227   m_match_index_ins = new int [m_max_nr_matches];
228 
229   m_temp_double = new double [12*DB_DEFAULT_NR_SAMPLES+10*m_max_nr_matches];
230   m_temp_int = new int [db_maxi(DB_DEFAULT_NR_SAMPLES,m_max_nr_matches)];
231 
232   // allocate space for homogenous image points:
233   m_corners_ref = new double [3*m_max_nr_corners];
234   m_corners_ins = new double [3*m_max_nr_corners];
235 
236   // allocate cost array and histogram:
237   m_sq_cost = new double [m_max_nr_matches];
238   m_cost_histogram = new int [m_nr_bins];
239 
240   // reserve array:
241   //m_inlier_indices.reserve(m_max_nr_matches);
242   m_inlier_indices = new int[m_max_nr_matches];
243 
244   m_initialized = true;
245 
246   m_max_inlier_count = 0;
247 }
248 
249 
250 #define MB 0
251 // Save the reference image, detect features and update the dref-to-ref transformation
UpdateReference(const unsigned char * const * im,bool subsample,bool detect_corners)252 int db_FrameToReferenceRegistration::UpdateReference(const unsigned char * const * im, bool subsample, bool detect_corners)
253 {
254   double temp[9];
255   db_Multiply3x3_3x3(temp,m_H_dref_to_ref,m_H_ref_to_ins);
256   db_Copy9(m_H_dref_to_ref,temp);
257 
258   const unsigned char * const * imptr = im;
259 
260   if (m_quarter_resolution && subsample)
261   {
262     GenerateQuarterResImage(im);
263     imptr = m_quarter_res_image;
264   }
265 
266   // save the reference image, detect features and quit
267   db_CopyImage_u(m_reference_image,imptr,m_im_width,m_im_height,m_over_allocation);
268 
269   if(detect_corners)
270   {
271     #if MB
272     m_cd.DetectCorners(imptr, m_x_corners_ref,m_y_corners_ref,&m_nr_corners_ref);
273     int nr = 0;
274     for(int k=0; k<m_nr_corners_ref; k++)
275     {
276         if(m_x_corners_ref[k]>m_im_width/3)
277         {
278             m_x_corners_ref[nr] = m_x_corners_ref[k];
279             m_y_corners_ref[nr] = m_y_corners_ref[k];
280             nr++;
281         }
282 
283     }
284     m_nr_corners_ref = nr;
285     #else
286     m_cd.DetectCorners(imptr, m_x_corners_ref,m_y_corners_ref,&m_nr_corners_ref);
287     #endif
288   }
289   else
290   {
291     m_nr_corners_ref = m_nr_corners_ins;
292 
293     for(int k=0; k<m_nr_corners_ins; k++)
294     {
295         m_x_corners_ref[k] = m_x_corners_ins[k];
296         m_y_corners_ref[k] = m_y_corners_ins[k];
297     }
298 
299   }
300 
301   db_Identity3x3(m_H_ref_to_ins);
302 
303   m_max_inlier_count = 0;   // Reset to 0 as no inliers seen until now
304   m_sq_cost_computed = false;
305   m_reference_set = true;
306   m_current_is_reference = true;
307   return 1;
308 }
309 
Get_H_dref_to_ref(double H[9])310 void db_FrameToReferenceRegistration::Get_H_dref_to_ref(double H[9])
311 {
312   db_Copy9(H,m_H_dref_to_ref);
313 }
314 
Get_H_dref_to_ins(double H[9])315 void db_FrameToReferenceRegistration::Get_H_dref_to_ins(double H[9])
316 {
317   db_Multiply3x3_3x3(H,m_H_dref_to_ref,m_H_ref_to_ins);
318 }
319 
Set_H_dref_to_ins(double H[9])320 void db_FrameToReferenceRegistration::Set_H_dref_to_ins(double H[9])
321 {
322     double H_ins_to_ref[9];
323 
324     db_Identity3x3(H_ins_to_ref);   // Ensure it has proper values
325     db_InvertAffineTransform(H_ins_to_ref,m_H_ref_to_ins);  // Invert to get ins to ref
326     db_Multiply3x3_3x3(m_H_dref_to_ref,H,H_ins_to_ref); // Update dref to ref using the input H from dref to ins
327 }
328 
329 
ResetDisplayReference()330 void db_FrameToReferenceRegistration::ResetDisplayReference()
331 {
332   db_Identity3x3(m_H_dref_to_ref);
333 }
334 
NeedReferenceUpdate()335 bool db_FrameToReferenceRegistration::NeedReferenceUpdate()
336 {
337   // If less than 50% of the starting number of inliers left, then its time to update the reference.
338   if(m_max_inlier_count>0 && float(m_num_inlier_indices)/float(m_max_inlier_count)<0.5)
339     return true;
340   else
341     return false;
342 }
343 
AddFrame(const unsigned char * const * im,double H[9],bool force_reference,bool prewarp)344 int db_FrameToReferenceRegistration::AddFrame(const unsigned char * const * im, double H[9],bool force_reference,bool prewarp)
345 {
346   m_current_is_reference = false;
347   if(!m_reference_set || force_reference)
348     {
349       db_Identity3x3(m_H_ref_to_ins);
350       db_Copy9(H,m_H_ref_to_ins);
351 
352       UpdateReference(im,true,true);
353       return 0;
354     }
355 
356   const unsigned char * const * imptr = im;
357 
358   if (m_quarter_resolution)
359   {
360     if (m_quarter_res_image)
361     {
362       GenerateQuarterResImage(im);
363     }
364 
365     imptr = (const unsigned char * const* )m_quarter_res_image;
366   }
367 
368   double H_last[9];
369   db_Copy9(H_last,m_H_ref_to_ins);
370   db_Identity3x3(m_H_ref_to_ins);
371 
372   m_sq_cost_computed = false;
373 
374   // detect corners on inspection image and match to reference image features:s
375 
376   // @jke - Adding code to time the functions.  TODO: Remove after test
377 #if PROFILE
378   double iTimer1, iTimer2;
379   char str[255];
380   strcpy(profile_string,"\n");
381   sprintf(str,"[%dx%d] %p\n",m_im_width,m_im_height,im);
382   strcat(profile_string, str);
383 #endif
384 
385   // @jke - Adding code to time the functions.  TODO: Remove after test
386 #if PROFILE
387   iTimer1 = now_ms();
388 #endif
389   m_cd.DetectCorners(imptr, m_x_corners_ins,m_y_corners_ins,&m_nr_corners_ins);
390   // @jke - Adding code to time the functions.  TODO: Remove after test
391 # if PROFILE
392   iTimer2 = now_ms();
393   double elapsedTimeCorner = iTimer2 - iTimer1;
394   sprintf(str,"Corner Detection [%d corners] = %g ms\n",m_nr_corners_ins, elapsedTimeCorner);
395   strcat(profile_string, str);
396 #endif
397 
398   // @jke - Adding code to time the functions.  TODO: Remove after test
399 #if PROFILE
400   iTimer1 = now_ms();
401 #endif
402     if(prewarp)
403   m_cm.Match(m_reference_image,imptr,m_x_corners_ref,m_y_corners_ref,m_nr_corners_ref,
404          m_x_corners_ins,m_y_corners_ins,m_nr_corners_ins,
405          m_match_index_ref,m_match_index_ins,&m_nr_matches,H,0);
406     else
407   m_cm.Match(m_reference_image,imptr,m_x_corners_ref,m_y_corners_ref,m_nr_corners_ref,
408          m_x_corners_ins,m_y_corners_ins,m_nr_corners_ins,
409          m_match_index_ref,m_match_index_ins,&m_nr_matches);
410   // @jke - Adding code to time the functions.  TODO: Remove after test
411 # if PROFILE
412   iTimer2 = now_ms();
413   double elapsedTimeMatch = iTimer2 - iTimer1;
414   sprintf(str,"Matching [%d] = %g ms\n",m_nr_matches,elapsedTimeMatch);
415   strcat(profile_string, str);
416 #endif
417 
418 
419   // copy out matching features:
420   for ( int i = 0; i < m_nr_matches; ++i )
421     {
422       int offset = 3*i;
423       m_corners_ref[offset  ] = m_x_corners_ref[m_match_index_ref[i]];
424       m_corners_ref[offset+1] = m_y_corners_ref[m_match_index_ref[i]];
425       m_corners_ref[offset+2] = 1.0;
426 
427       m_corners_ins[offset  ] = m_x_corners_ins[m_match_index_ins[i]];
428       m_corners_ins[offset+1] = m_y_corners_ins[m_match_index_ins[i]];
429       m_corners_ins[offset+2] = 1.0;
430     }
431 
432   // @jke - Adding code to time the functions.  TODO: Remove after test
433 #if PROFILE
434   iTimer1 = now_ms();
435 #endif
436   // perform the alignment:
437   db_RobImageHomography(m_H_ref_to_ins, m_corners_ref, m_corners_ins, m_nr_matches, m_K, m_K, m_temp_double, m_temp_int,
438             m_homography_type,NULL,m_max_iterations,m_max_nr_matches,m_scale,
439             m_nr_samples, m_chunk_size);
440   // @jke - Adding code to time the functions.  TODO: Remove after test
441 # if PROFILE
442   iTimer2 = now_ms();
443   double elapsedTimeHomography = iTimer2 - iTimer1;
444   sprintf(str,"Homography = %g ms\n",elapsedTimeHomography);
445   strcat(profile_string, str);
446 #endif
447 
448 
449   SetOutlierThreshold();
450 
451   // Compute the inliers for the db compute m_H_ref_to_ins
452   ComputeInliers(m_H_ref_to_ins);
453 
454   // Update the max inlier count
455   m_max_inlier_count = (m_max_inlier_count > m_num_inlier_indices)?m_max_inlier_count:m_num_inlier_indices;
456 
457   // Fit a least-squares model to just the inliers and put it in m_H_ref_to_ins
458   if(m_linear_polish)
459     Polish(m_inlier_indices, m_num_inlier_indices);
460 
461   if (m_quarter_resolution)
462   {
463     m_H_ref_to_ins[2] *= 2.0;
464     m_H_ref_to_ins[5] *= 2.0;
465   }
466 
467 #if PROFILE
468   sprintf(str,"#Inliers = %d \n",m_num_inlier_indices);
469   strcat(profile_string, str);
470 #endif
471 /*
472   ///// CHECK IF CURRENT TRANSFORMATION GOOD OR BAD ////
473   ///// IF BAD, then update reference to the last correctly aligned inspection frame;
474   if(m_num_inlier_indices<5)//0.9*m_nr_matches || m_nr_matches < 20)
475   {
476     db_Copy9(m_H_ref_to_ins,H_last);
477     UpdateReference(imptr,false);
478 //  UpdateReference(m_aligned_ins_image,false);
479   }
480   else
481   {
482   ///// IF GOOD, then update the last correctly aligned inspection frame to be this;
483   //db_CopyImage_u(m_aligned_ins_image,imptr,m_im_width,m_im_height,m_over_allocation);
484 */
485   if(m_do_motion_smoothing)
486     SmoothMotion();
487 
488    db_PrintDoubleMatrix(m_H_ref_to_ins,3,3);
489 
490   db_Copy9(H, m_H_ref_to_ins);
491 
492   m_nr_frames_processed++;
493 {
494   if ( (m_nr_frames_processed % m_reference_update_period) == 0 )
495   {
496     //UpdateReference(imptr,false, false);
497 
498     #if MB
499     UpdateReference(imptr,false, true);
500     #else
501     UpdateReference(imptr,false, false);
502     #endif
503   }
504 
505 
506   }
507 
508 
509 
510   return 1;
511 }
512 
513 //void db_FrameToReferenceRegistration::ComputeInliers(double H[9],std::vector<int> &inlier_indices)
ComputeInliers(double H[9])514 void db_FrameToReferenceRegistration::ComputeInliers(double H[9])
515 {
516   double totnummatches = m_nr_matches;
517   int inliercount=0;
518 
519   m_num_inlier_indices = 0;
520 //  inlier_indices.clear();
521 
522   for(int c=0; c < totnummatches; c++ )
523     {
524       if (m_sq_cost[c] <= m_outlier_t2)
525     {
526       m_inlier_indices[inliercount] = c;
527       inliercount++;
528     }
529     }
530 
531   m_num_inlier_indices = inliercount;
532   double frac=inliercount/totnummatches;
533 }
534 
535 //void db_FrameToReferenceRegistration::Polish(std::vector<int> &inlier_indices)
Polish(int * inlier_indices,int & num_inlier_indices)536 void db_FrameToReferenceRegistration::Polish(int *inlier_indices, int &num_inlier_indices)
537 {
538   db_Zero(m_polish_C,36);
539   db_Zero(m_polish_D,6);
540   for (int i=0;i<num_inlier_indices;i++)
541     {
542       int j = 3*inlier_indices[i];
543       m_polish_C[0]+=m_corners_ref[j]*m_corners_ref[j];
544       m_polish_C[1]+=m_corners_ref[j]*m_corners_ref[j+1];
545       m_polish_C[2]+=m_corners_ref[j];
546       m_polish_C[7]+=m_corners_ref[j+1]*m_corners_ref[j+1];
547       m_polish_C[8]+=m_corners_ref[j+1];
548       m_polish_C[14]+=1;
549       m_polish_D[0]+=m_corners_ref[j]*m_corners_ins[j];
550       m_polish_D[1]+=m_corners_ref[j+1]*m_corners_ins[j];
551       m_polish_D[2]+=m_corners_ins[j];
552       m_polish_D[3]+=m_corners_ref[j]*m_corners_ins[j+1];
553       m_polish_D[4]+=m_corners_ref[j+1]*m_corners_ins[j+1];
554       m_polish_D[5]+=m_corners_ins[j+1];
555     }
556 
557   double a=db_maxd(m_polish_C[0],m_polish_C[7]);
558   m_polish_C[0]/=a; m_polish_C[1]/=a;   m_polish_C[2]/=a;
559   m_polish_C[7]/=a; m_polish_C[8]/=a; m_polish_C[14]/=a;
560 
561   m_polish_D[0]/=a; m_polish_D[1]/=a;   m_polish_D[2]/=a;
562   m_polish_D[3]/=a; m_polish_D[4]/=a;   m_polish_D[5]/=a;
563 
564 
565   m_polish_C[6]=m_polish_C[1];
566   m_polish_C[12]=m_polish_C[2];
567   m_polish_C[13]=m_polish_C[8];
568 
569   m_polish_C[21]=m_polish_C[0]; m_polish_C[22]=m_polish_C[1]; m_polish_C[23]=m_polish_C[2];
570   m_polish_C[28]=m_polish_C[7]; m_polish_C[29]=m_polish_C[8];
571   m_polish_C[35]=m_polish_C[14];
572 
573 
574   double d[6];
575   db_CholeskyDecomp6x6(m_polish_C,d);
576   db_CholeskyBacksub6x6(m_H_ref_to_ins,m_polish_C,d,m_polish_D);
577 }
578 
EstimateSecondaryModel(double H[9])579 void db_FrameToReferenceRegistration::EstimateSecondaryModel(double H[9])
580 {
581   /*      if ( m_current_is_reference )
582       {
583       db_Identity3x3(H);
584       return;
585       }
586   */
587 
588   // select the outliers of the current model:
589   SelectOutliers();
590 
591   // perform the alignment:
592   db_RobImageHomography(m_H_ref_to_ins, m_corners_ref, m_corners_ins, m_nr_matches, m_K, m_K, m_temp_double, m_temp_int,
593             m_homography_type,NULL,m_max_iterations,m_max_nr_matches,m_scale,
594             m_nr_samples, m_chunk_size);
595 
596   db_Copy9(H,m_H_ref_to_ins);
597 }
598 
ComputeCostArray()599 void db_FrameToReferenceRegistration::ComputeCostArray()
600 {
601   if ( m_sq_cost_computed ) return;
602 
603   for( int c=0, k=0 ;c < m_nr_matches; c++, k=k+3)
604     {
605       m_sq_cost[c] = SquaredInhomogenousHomographyError(m_corners_ins+k,m_H_ref_to_ins,m_corners_ref+k);
606     }
607 
608   m_sq_cost_computed = true;
609 }
610 
SelectOutliers()611 void db_FrameToReferenceRegistration::SelectOutliers()
612 {
613   int nr_outliers=0;
614 
615   ComputeCostArray();
616 
617   for(int c=0, k=0 ;c<m_nr_matches;c++,k=k+3)
618     {
619       if (m_sq_cost[c] > m_outlier_t2)
620     {
621       int offset = 3*nr_outliers++;
622       db_Copy3(m_corners_ref+offset,m_corners_ref+k);
623       db_Copy3(m_corners_ins+offset,m_corners_ins+k);
624     }
625     }
626 
627   m_nr_matches = nr_outliers;
628 }
629 
ComputeCostHistogram()630 void db_FrameToReferenceRegistration::ComputeCostHistogram()
631 {
632   ComputeCostArray();
633 
634   for ( int b = 0; b < m_nr_bins; ++b )
635     m_cost_histogram[b] = 0;
636 
637   for(int c = 0; c < m_nr_matches; c++)
638     {
639       double error = db_SafeSqrt(m_sq_cost[c]);
640       int bin = (int)(error/m_max_cost_pix*m_nr_bins);
641       if ( bin < m_nr_bins )
642     m_cost_histogram[bin]++;
643       else
644     m_cost_histogram[m_nr_bins-1]++;
645     }
646 
647 /*
648   for ( int i = 0; i < m_nr_bins; ++i )
649     std::cout << m_cost_histogram[i] << " ";
650   std::cout << std::endl;
651 */
652 }
653 
SetOutlierThreshold()654 void db_FrameToReferenceRegistration::SetOutlierThreshold()
655 {
656   ComputeCostHistogram();
657 
658   int i = 0, last=0;
659   for (; i < m_nr_bins-1; ++i )
660     {
661       if ( last > m_cost_histogram[i] )
662     break;
663       last = m_cost_histogram[i];
664     }
665 
666   //std::cout << "I " <<  i << std::endl;
667 
668   int max = m_cost_histogram[i];
669 
670   for (; i < m_nr_bins-1; ++i )
671     {
672       if ( m_cost_histogram[i] < (int)(0.1*max) )
673     //if ( last < m_cost_histogram[i] )
674     break;
675       last = m_cost_histogram[i];
676     }
677   //std::cout << "J " <<  i << std::endl;
678 
679   m_outlier_t2 = db_sqr(i*m_max_cost_pix/m_nr_bins);
680 
681   //std::cout << "m_outlier_t2 " <<  m_outlier_t2 << std::endl;
682 }
683 
SmoothMotion(void)684 void db_FrameToReferenceRegistration::SmoothMotion(void)
685 {
686   VP_MOTION inmot,outmot;
687 
688   double H[9];
689 
690   Get_H_dref_to_ins(H);
691 
692       MXX(inmot) = H[0];
693     MXY(inmot) = H[1];
694     MXZ(inmot) = H[2];
695     MXW(inmot) = 0.0;
696 
697     MYX(inmot) = H[3];
698     MYY(inmot) = H[4];
699     MYZ(inmot) = H[5];
700     MYW(inmot) = 0.0;
701 
702     MZX(inmot) = H[6];
703     MZY(inmot) = H[7];
704     MZZ(inmot) = H[8];
705     MZW(inmot) = 0.0;
706 
707     MWX(inmot) = 0.0;
708     MWY(inmot) = 0.0;
709     MWZ(inmot) = 0.0;
710     MWW(inmot) = 1.0;
711 
712     inmot.type = VP_MOTION_AFFINE;
713 
714     int w = m_im_width;
715     int h = m_im_height;
716 
717     if(m_quarter_resolution)
718     {
719     w = w*2;
720     h = h*2;
721     }
722 
723 #if 0
724     m_stab_smoother.smoothMotionAdaptive(w,h,&inmot,&outmot);
725 #else
726     m_stab_smoother.smoothMotion(&inmot,&outmot);
727 #endif
728 
729     H[0] = MXX(outmot);
730     H[1] = MXY(outmot);
731     H[2] = MXZ(outmot);
732 
733     H[3] = MYX(outmot);
734     H[4] = MYY(outmot);
735     H[5] = MYZ(outmot);
736 
737     H[6] = MZX(outmot);
738     H[7] = MZY(outmot);
739     H[8] = MZZ(outmot);
740 
741     Set_H_dref_to_ins(H);
742 }
743 
GenerateQuarterResImage(const unsigned char * const * im)744 void db_FrameToReferenceRegistration::GenerateQuarterResImage(const unsigned char* const* im)
745 {
746   int input_h = m_im_height*2;
747   int input_w = m_im_width*2;
748 
749   for (int j = 0; j < input_h; j++)
750   {
751     const unsigned char* in_row_ptr = im[j];
752     unsigned char* out_row_ptr = m_horz_smooth_subsample_image[j]+1;
753 
754     for (int i = 2; i < input_w-2; i += 2)
755     {
756       int smooth_val = (
757             6*in_row_ptr[i] +
758             ((in_row_ptr[i-1]+in_row_ptr[i+1])<<2) +
759             in_row_ptr[i-2]+in_row_ptr[i+2]
760             ) >> 4;
761       *out_row_ptr++ = (unsigned char) smooth_val;
762 
763       if ( (smooth_val < 0) || (smooth_val > 255))
764       {
765         return;
766       }
767 
768     }
769   }
770 
771   for (int j = 2; j < input_h-2; j+=2)
772   {
773 
774     unsigned char* in_row_ptr = m_horz_smooth_subsample_image[j];
775     unsigned char* out_row_ptr = m_quarter_res_image[j/2];
776 
777     for (int i = 1; i < m_im_width-1; i++)
778     {
779       int smooth_val = (
780             6*in_row_ptr[i] +
781             ((in_row_ptr[i-m_im_width]+in_row_ptr[i+m_im_width]) << 2)+
782             in_row_ptr[i-2*m_im_width]+in_row_ptr[i+2*m_im_width]
783             ) >> 4;
784       *out_row_ptr++ = (unsigned char)smooth_val;
785 
786       if ( (smooth_val < 0) || (smooth_val > 255))
787       {
788         return;
789       }
790 
791     }
792   }
793 }
794