Home | History | Annotate | Download | only in dbreg
      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 
     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 
     74 db_FrameToReferenceRegistration::~db_FrameToReferenceRegistration()
     75 {
     76   Clean();
     77 }
     78 
     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 
    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
    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 
    310 void db_FrameToReferenceRegistration::Get_H_dref_to_ref(double H[9])
    311 {
    312   db_Copy9(H,m_H_dref_to_ref);
    313 }
    314 
    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 
    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 
    330 void db_FrameToReferenceRegistration::ResetDisplayReference()
    331 {
    332   db_Identity3x3(m_H_dref_to_ref);
    333 }
    334 
    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 
    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)
    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)
    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 
    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 
    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 
    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 
    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 
    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 
    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 
    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