summaryrefslogtreecommitdiffstats
path: root/jni/feature_stab/src/dbreg/dbreg.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'jni/feature_stab/src/dbreg/dbreg.cpp')
-rw-r--r--jni/feature_stab/src/dbreg/dbreg.cpp795
1 files changed, 795 insertions, 0 deletions
diff --git a/jni/feature_stab/src/dbreg/dbreg.cpp b/jni/feature_stab/src/dbreg/dbreg.cpp
new file mode 100644
index 0000000..b87cbbd
--- /dev/null
+++ b/jni/feature_stab/src/dbreg/dbreg.cpp
@@ -0,0 +1,795 @@
+/*
+ * Copyright (C) 2011 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+// $Id: dbreg.cpp,v 1.31 2011/06/17 14:04:32 mbansal Exp $
+#include "dbreg.h"
+#include <string.h>
+#include <stdio.h>
+
+
+#if PROFILE
+#endif
+
+//#include <iostream>
+
+db_FrameToReferenceRegistration::db_FrameToReferenceRegistration() :
+ m_initialized(false),m_nr_matches(0),m_over_allocation(256),m_nr_bins(20),m_max_cost_pix(30), m_quarter_resolution(false)
+{
+ m_reference_image = NULL;
+ m_aligned_ins_image = NULL;
+
+ m_quarter_res_image = NULL;
+ m_horz_smooth_subsample_image = NULL;
+
+ m_x_corners_ref = NULL;
+ m_y_corners_ref = NULL;
+
+ m_x_corners_ins = NULL;
+ m_y_corners_ins = NULL;
+
+ m_match_index_ref = NULL;
+ m_match_index_ins = NULL;
+
+ m_inlier_indices = NULL;
+
+ m_num_inlier_indices = 0;
+
+ m_temp_double = NULL;
+ m_temp_int = NULL;
+
+ m_corners_ref = NULL;
+ m_corners_ins = NULL;
+
+ m_sq_cost = NULL;
+ m_cost_histogram = NULL;
+
+ profile_string = NULL;
+
+ db_Identity3x3(m_K);
+ db_Identity3x3(m_H_ref_to_ins);
+ db_Identity3x3(m_H_dref_to_ref);
+
+ m_sq_cost_computed = false;
+ m_reference_set = false;
+
+ m_reference_update_period = 0;
+ m_nr_frames_processed = 0;
+
+ return;
+}
+
+db_FrameToReferenceRegistration::~db_FrameToReferenceRegistration()
+{
+ Clean();
+}
+
+void db_FrameToReferenceRegistration::Clean()
+{
+ if ( m_reference_image )
+ db_FreeImage_u(m_reference_image,m_im_height);
+
+ if ( m_aligned_ins_image )
+ db_FreeImage_u(m_aligned_ins_image,m_im_height);
+
+ if ( m_quarter_res_image )
+ {
+ db_FreeImage_u(m_quarter_res_image, m_im_height);
+ }
+
+ if ( m_horz_smooth_subsample_image )
+ {
+ db_FreeImage_u(m_horz_smooth_subsample_image, m_im_height*2);
+ }
+
+ delete [] m_x_corners_ref;
+ delete [] m_y_corners_ref;
+
+ delete [] m_x_corners_ins;
+ delete [] m_y_corners_ins;
+
+ delete [] m_match_index_ref;
+ delete [] m_match_index_ins;
+
+ delete [] m_temp_double;
+ delete [] m_temp_int;
+
+ delete [] m_corners_ref;
+ delete [] m_corners_ins;
+
+ delete [] m_sq_cost;
+ delete [] m_cost_histogram;
+
+ delete [] m_inlier_indices;
+
+ if(profile_string)
+ delete [] profile_string;
+
+ m_reference_image = NULL;
+ m_aligned_ins_image = NULL;
+
+ m_quarter_res_image = NULL;
+ m_horz_smooth_subsample_image = NULL;
+
+ m_x_corners_ref = NULL;
+ m_y_corners_ref = NULL;
+
+ m_x_corners_ins = NULL;
+ m_y_corners_ins = NULL;
+
+ m_match_index_ref = NULL;
+ m_match_index_ins = NULL;
+
+ m_inlier_indices = NULL;
+
+ m_temp_double = NULL;
+ m_temp_int = NULL;
+
+ m_corners_ref = NULL;
+ m_corners_ins = NULL;
+
+ m_sq_cost = NULL;
+ m_cost_histogram = NULL;
+}
+
+void db_FrameToReferenceRegistration::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
+ )
+{
+ Clean();
+
+ m_reference_update_period = reference_update_period;
+ m_nr_frames_processed = 0;
+
+ m_do_motion_smoothing = do_motion_smoothing;
+ m_motion_smoothing_gain = motion_smoothing_gain;
+
+ m_stab_smoother.setSmoothingFactor(m_motion_smoothing_gain);
+
+ m_quarter_resolution = quarter_resolution;
+
+ profile_string = new char[10240];
+
+ if (m_quarter_resolution == true)
+ {
+ width = width/2;
+ height = height/2;
+
+ m_horz_smooth_subsample_image = db_AllocImage_u(width,height*2,m_over_allocation);
+ m_quarter_res_image = db_AllocImage_u(width,height,m_over_allocation);
+ }
+
+ m_im_width = width;
+ m_im_height = height;
+
+ double temp[9];
+ db_Approx3DCalMat(m_K,temp,m_im_width,m_im_height);
+
+ m_homography_type = homography_type;
+ m_max_iterations = max_iterations;
+ m_scale = 2/(m_K[0]+m_K[4]);
+ m_nr_samples = nr_samples;
+ m_chunk_size = chunk_size;
+
+ double outlier_t1 = 5.0;
+
+ m_outlier_t2 = outlier_t1*outlier_t1;//*m_scale*m_scale;
+
+ m_current_is_reference = false;
+
+ m_linear_polish = linear_polish;
+
+ m_reference_image = db_AllocImage_u(m_im_width,m_im_height,m_over_allocation);
+ m_aligned_ins_image = db_AllocImage_u(m_im_width,m_im_height,m_over_allocation);
+
+ // initialize feature detection and matching:
+ //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);
+ 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);
+
+ int use_21 = 0;
+ 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);
+
+ // allocate space for corner feature locations for reference and inspection images:
+ m_x_corners_ref = new double [m_max_nr_corners];
+ m_y_corners_ref = new double [m_max_nr_corners];
+
+ m_x_corners_ins = new double [m_max_nr_corners];
+ m_y_corners_ins = new double [m_max_nr_corners];
+
+ // allocate space for match indices:
+ m_match_index_ref = new int [m_max_nr_matches];
+ m_match_index_ins = new int [m_max_nr_matches];
+
+ m_temp_double = new double [12*DB_DEFAULT_NR_SAMPLES+10*m_max_nr_matches];
+ m_temp_int = new int [db_maxi(DB_DEFAULT_NR_SAMPLES,m_max_nr_matches)];
+
+ // allocate space for homogenous image points:
+ m_corners_ref = new double [3*m_max_nr_corners];
+ m_corners_ins = new double [3*m_max_nr_corners];
+
+ // allocate cost array and histogram:
+ m_sq_cost = new double [m_max_nr_matches];
+ m_cost_histogram = new int [m_nr_bins];
+
+ // reserve array:
+ //m_inlier_indices.reserve(m_max_nr_matches);
+ m_inlier_indices = new int[m_max_nr_matches];
+
+ m_initialized = true;
+
+ m_max_inlier_count = 0;
+}
+
+
+#define MB 0
+// Save the reference image, detect features and update the dref-to-ref transformation
+int db_FrameToReferenceRegistration::UpdateReference(const unsigned char * const * im, bool subsample, bool detect_corners)
+{
+ double temp[9];
+ db_Multiply3x3_3x3(temp,m_H_dref_to_ref,m_H_ref_to_ins);
+ db_Copy9(m_H_dref_to_ref,temp);
+
+ const unsigned char * const * imptr = im;
+
+ if (m_quarter_resolution && subsample)
+ {
+ GenerateQuarterResImage(im);
+ imptr = m_quarter_res_image;
+ }
+
+ // save the reference image, detect features and quit
+ db_CopyImage_u(m_reference_image,imptr,m_im_width,m_im_height,m_over_allocation);
+
+ if(detect_corners)
+ {
+ #if MB
+ m_cd.DetectCorners(imptr, m_x_corners_ref,m_y_corners_ref,&m_nr_corners_ref);
+ int nr = 0;
+ for(int k=0; k<m_nr_corners_ref; k++)
+ {
+ if(m_x_corners_ref[k]>m_im_width/3)
+ {
+ m_x_corners_ref[nr] = m_x_corners_ref[k];
+ m_y_corners_ref[nr] = m_y_corners_ref[k];
+ nr++;
+ }
+
+ }
+ m_nr_corners_ref = nr;
+ #else
+ m_cd.DetectCorners(imptr, m_x_corners_ref,m_y_corners_ref,&m_nr_corners_ref);
+ #endif
+ }
+ else
+ {
+ m_nr_corners_ref = m_nr_corners_ins;
+
+ for(int k=0; k<m_nr_corners_ins; k++)
+ {
+ m_x_corners_ref[k] = m_x_corners_ins[k];
+ m_y_corners_ref[k] = m_y_corners_ins[k];
+ }
+
+ }
+
+ db_Identity3x3(m_H_ref_to_ins);
+
+ m_max_inlier_count = 0; // Reset to 0 as no inliers seen until now
+ m_sq_cost_computed = false;
+ m_reference_set = true;
+ m_current_is_reference = true;
+ return 1;
+}
+
+void db_FrameToReferenceRegistration::Get_H_dref_to_ref(double H[9])
+{
+ db_Copy9(H,m_H_dref_to_ref);
+}
+
+void db_FrameToReferenceRegistration::Get_H_dref_to_ins(double H[9])
+{
+ db_Multiply3x3_3x3(H,m_H_dref_to_ref,m_H_ref_to_ins);
+}
+
+void db_FrameToReferenceRegistration::Set_H_dref_to_ins(double H[9])
+{
+ double H_ins_to_ref[9];
+
+ db_Identity3x3(H_ins_to_ref); // Ensure it has proper values
+ db_InvertAffineTransform(H_ins_to_ref,m_H_ref_to_ins); // Invert to get ins to ref
+ 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
+}
+
+
+void db_FrameToReferenceRegistration::ResetDisplayReference()
+{
+ db_Identity3x3(m_H_dref_to_ref);
+}
+
+bool db_FrameToReferenceRegistration::NeedReferenceUpdate()
+{
+ // If less than 50% of the starting number of inliers left, then its time to update the reference.
+ if(m_max_inlier_count>0 && float(m_num_inlier_indices)/float(m_max_inlier_count)<0.5)
+ return true;
+ else
+ return false;
+}
+
+int db_FrameToReferenceRegistration::AddFrame(const unsigned char * const * im, double H[9],bool force_reference,bool prewarp)
+{
+ m_current_is_reference = false;
+ if(!m_reference_set)
+ {
+ db_Identity3x3(m_H_ref_to_ins);
+ db_Copy9(H,m_H_ref_to_ins);
+
+ UpdateReference(im,true,true);
+ return 0;
+ }
+
+ const unsigned char * const * imptr = im;
+
+ if (m_quarter_resolution)
+ {
+ if (m_quarter_res_image)
+ {
+ GenerateQuarterResImage(im);
+ }
+
+ imptr = (const unsigned char * const* )m_quarter_res_image;
+ }
+
+ double H_last[9];
+ db_Copy9(H_last,m_H_ref_to_ins);
+ db_Identity3x3(m_H_ref_to_ins);
+
+ m_sq_cost_computed = false;
+
+ // detect corners on inspection image and match to reference image features:s
+
+ // @jke - Adding code to time the functions. TODO: Remove after test
+#if PROFILE
+ double iTimer1, iTimer2;
+ char str[255];
+ strcpy(profile_string,"\n");
+ sprintf(str,"[%dx%d] %p\n",m_im_width,m_im_height,im);
+ strcat(profile_string, str);
+#endif
+
+ // @jke - Adding code to time the functions. TODO: Remove after test
+#if PROFILE
+ iTimer1 = now_ms();
+#endif
+ m_cd.DetectCorners(imptr, m_x_corners_ins,m_y_corners_ins,&m_nr_corners_ins);
+ // @jke - Adding code to time the functions. TODO: Remove after test
+# if PROFILE
+ iTimer2 = now_ms();
+ double elapsedTimeCorner = iTimer2 - iTimer1;
+ sprintf(str,"Corner Detection [%d corners] = %g ms\n",m_nr_corners_ins, elapsedTimeCorner);
+ strcat(profile_string, str);
+#endif
+
+ // @jke - Adding code to time the functions. TODO: Remove after test
+#if PROFILE
+ iTimer1 = now_ms();
+#endif
+ if(prewarp)
+ m_cm.Match(m_reference_image,imptr,m_x_corners_ref,m_y_corners_ref,m_nr_corners_ref,
+ m_x_corners_ins,m_y_corners_ins,m_nr_corners_ins,
+ m_match_index_ref,m_match_index_ins,&m_nr_matches,H,0);
+ else
+ m_cm.Match(m_reference_image,imptr,m_x_corners_ref,m_y_corners_ref,m_nr_corners_ref,
+ m_x_corners_ins,m_y_corners_ins,m_nr_corners_ins,
+ m_match_index_ref,m_match_index_ins,&m_nr_matches);
+ // @jke - Adding code to time the functions. TODO: Remove after test
+# if PROFILE
+ iTimer2 = now_ms();
+ double elapsedTimeMatch = iTimer2 - iTimer1;
+ sprintf(str,"Matching [%d] = %g ms\n",m_nr_matches,elapsedTimeMatch);
+ strcat(profile_string, str);
+#endif
+
+
+ // copy out matching features:
+ for ( int i = 0; i < m_nr_matches; ++i )
+ {
+ int offset = 3*i;
+ m_corners_ref[offset ] = m_x_corners_ref[m_match_index_ref[i]];
+ m_corners_ref[offset+1] = m_y_corners_ref[m_match_index_ref[i]];
+ m_corners_ref[offset+2] = 1.0;
+
+ m_corners_ins[offset ] = m_x_corners_ins[m_match_index_ins[i]];
+ m_corners_ins[offset+1] = m_y_corners_ins[m_match_index_ins[i]];
+ m_corners_ins[offset+2] = 1.0;
+ }
+
+ // @jke - Adding code to time the functions. TODO: Remove after test
+#if PROFILE
+ iTimer1 = now_ms();
+#endif
+ // perform the alignment:
+ 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,
+ m_homography_type,NULL,m_max_iterations,m_max_nr_matches,m_scale,
+ m_nr_samples, m_chunk_size);
+ // @jke - Adding code to time the functions. TODO: Remove after test
+# if PROFILE
+ iTimer2 = now_ms();
+ double elapsedTimeHomography = iTimer2 - iTimer1;
+ sprintf(str,"Homography = %g ms\n",elapsedTimeHomography);
+ strcat(profile_string, str);
+#endif
+
+
+ SetOutlierThreshold();
+
+ // Compute the inliers for the db compute m_H_ref_to_ins
+ ComputeInliers(m_H_ref_to_ins);
+
+ // Update the max inlier count
+ m_max_inlier_count = (m_max_inlier_count > m_num_inlier_indices)?m_max_inlier_count:m_num_inlier_indices;
+
+ // Fit a least-squares model to just the inliers and put it in m_H_ref_to_ins
+ if(m_linear_polish)
+ Polish(m_inlier_indices, m_num_inlier_indices);
+
+ if (m_quarter_resolution)
+ {
+ m_H_ref_to_ins[2] *= 2.0;
+ m_H_ref_to_ins[5] *= 2.0;
+ }
+
+#if PROFILE
+ sprintf(str,"#Inliers = %d \n",m_num_inlier_indices);
+ strcat(profile_string, str);
+#endif
+/*
+ ///// CHECK IF CURRENT TRANSFORMATION GOOD OR BAD ////
+ ///// IF BAD, then update reference to the last correctly aligned inspection frame;
+ if(m_num_inlier_indices<5)//0.9*m_nr_matches || m_nr_matches < 20)
+ {
+ db_Copy9(m_H_ref_to_ins,H_last);
+ UpdateReference(imptr,false);
+// UpdateReference(m_aligned_ins_image,false);
+ }
+ else
+ {
+ ///// IF GOOD, then update the last correctly aligned inspection frame to be this;
+ //db_CopyImage_u(m_aligned_ins_image,imptr,m_im_width,m_im_height,m_over_allocation);
+*/
+ if(m_do_motion_smoothing)
+ SmoothMotion();
+
+ db_PrintDoubleMatrix(m_H_ref_to_ins,3,3);
+
+ db_Copy9(H, m_H_ref_to_ins);
+
+ m_nr_frames_processed++;
+{
+ if ( (m_nr_frames_processed % m_reference_update_period) == 0 )
+ {
+ //UpdateReference(imptr,false, false);
+
+ #if MB
+ UpdateReference(imptr,false, true);
+ #else
+ UpdateReference(imptr,false, false);
+ #endif
+ }
+
+
+ }
+
+
+
+ return 1;
+}
+
+//void db_FrameToReferenceRegistration::ComputeInliers(double H[9],std::vector<int> &inlier_indices)
+void db_FrameToReferenceRegistration::ComputeInliers(double H[9])
+{
+ double totnummatches = m_nr_matches;
+ int inliercount=0;
+
+ m_num_inlier_indices = 0;
+// inlier_indices.clear();
+
+ for(int c=0; c < totnummatches; c++ )
+ {
+ if (m_sq_cost[c] <= m_outlier_t2)
+ {
+ m_inlier_indices[inliercount] = c;
+ inliercount++;
+ }
+ }
+
+ m_num_inlier_indices = inliercount;
+ double frac=inliercount/totnummatches;
+}
+
+//void db_FrameToReferenceRegistration::Polish(std::vector<int> &inlier_indices)
+void db_FrameToReferenceRegistration::Polish(int *inlier_indices, int &num_inlier_indices)
+{
+ db_Zero(m_polish_C,36);
+ db_Zero(m_polish_D,6);
+ for (int i=0;i<num_inlier_indices;i++)
+ {
+ int j = 3*inlier_indices[i];
+ m_polish_C[0]+=m_corners_ref[j]*m_corners_ref[j];
+ m_polish_C[1]+=m_corners_ref[j]*m_corners_ref[j+1];
+ m_polish_C[2]+=m_corners_ref[j];
+ m_polish_C[7]+=m_corners_ref[j+1]*m_corners_ref[j+1];
+ m_polish_C[8]+=m_corners_ref[j+1];
+ m_polish_C[14]+=1;
+ m_polish_D[0]+=m_corners_ref[j]*m_corners_ins[j];
+ m_polish_D[1]+=m_corners_ref[j+1]*m_corners_ins[j];
+ m_polish_D[2]+=m_corners_ins[j];
+ m_polish_D[3]+=m_corners_ref[j]*m_corners_ins[j+1];
+ m_polish_D[4]+=m_corners_ref[j+1]*m_corners_ins[j+1];
+ m_polish_D[5]+=m_corners_ins[j+1];
+ }
+
+ double a=db_maxd(m_polish_C[0],m_polish_C[7]);
+ m_polish_C[0]/=a; m_polish_C[1]/=a; m_polish_C[2]/=a;
+ m_polish_C[7]/=a; m_polish_C[8]/=a; m_polish_C[14]/=a;
+
+ m_polish_D[0]/=a; m_polish_D[1]/=a; m_polish_D[2]/=a;
+ m_polish_D[3]/=a; m_polish_D[4]/=a; m_polish_D[5]/=a;
+
+
+ m_polish_C[6]=m_polish_C[1];
+ m_polish_C[12]=m_polish_C[2];
+ m_polish_C[13]=m_polish_C[8];
+
+ m_polish_C[21]=m_polish_C[0]; m_polish_C[22]=m_polish_C[1]; m_polish_C[23]=m_polish_C[2];
+ m_polish_C[28]=m_polish_C[7]; m_polish_C[29]=m_polish_C[8];
+ m_polish_C[35]=m_polish_C[14];
+
+
+ double d[6];
+ db_CholeskyDecomp6x6(m_polish_C,d);
+ db_CholeskyBacksub6x6(m_H_ref_to_ins,m_polish_C,d,m_polish_D);
+}
+
+void db_FrameToReferenceRegistration::EstimateSecondaryModel(double H[9])
+{
+ /* if ( m_current_is_reference )
+ {
+ db_Identity3x3(H);
+ return;
+ }
+ */
+
+ // select the outliers of the current model:
+ SelectOutliers();
+
+ // perform the alignment:
+ 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,
+ m_homography_type,NULL,m_max_iterations,m_max_nr_matches,m_scale,
+ m_nr_samples, m_chunk_size);
+
+ db_Copy9(H,m_H_ref_to_ins);
+}
+
+void db_FrameToReferenceRegistration::ComputeCostArray()
+{
+ if ( m_sq_cost_computed ) return;
+
+ for( int c=0, k=0 ;c < m_nr_matches; c++, k=k+3)
+ {
+ m_sq_cost[c] = SquaredInhomogenousHomographyError(m_corners_ins+k,m_H_ref_to_ins,m_corners_ref+k);
+ }
+
+ m_sq_cost_computed = true;
+}
+
+void db_FrameToReferenceRegistration::SelectOutliers()
+{
+ int nr_outliers=0;
+
+ ComputeCostArray();
+
+ for(int c=0, k=0 ;c<m_nr_matches;c++,k=k+3)
+ {
+ if (m_sq_cost[c] > m_outlier_t2)
+ {
+ int offset = 3*nr_outliers++;
+ db_Copy3(m_corners_ref+offset,m_corners_ref+k);
+ db_Copy3(m_corners_ins+offset,m_corners_ins+k);
+ }
+ }
+
+ m_nr_matches = nr_outliers;
+}
+
+void db_FrameToReferenceRegistration::ComputeCostHistogram()
+{
+ ComputeCostArray();
+
+ for ( int b = 0; b < m_nr_bins; ++b )
+ m_cost_histogram[b] = 0;
+
+ for(int c = 0; c < m_nr_matches; c++)
+ {
+ double error = db_SafeSqrt(m_sq_cost[c]);
+ int bin = (int)(error/m_max_cost_pix*m_nr_bins);
+ if ( bin < m_nr_bins )
+ m_cost_histogram[bin]++;
+ else
+ m_cost_histogram[m_nr_bins-1]++;
+ }
+
+/*
+ for ( int i = 0; i < m_nr_bins; ++i )
+ std::cout << m_cost_histogram[i] << " ";
+ std::cout << std::endl;
+*/
+}
+
+void db_FrameToReferenceRegistration::SetOutlierThreshold()
+{
+ ComputeCostHistogram();
+
+ int i = 0, last=0;
+ for (; i < m_nr_bins-1; ++i )
+ {
+ if ( last > m_cost_histogram[i] )
+ break;
+ last = m_cost_histogram[i];
+ }
+
+ //std::cout << "I " << i << std::endl;
+
+ int max = m_cost_histogram[i];
+
+ for (; i < m_nr_bins-1; ++i )
+ {
+ if ( m_cost_histogram[i] < (int)(0.1*max) )
+ //if ( last < m_cost_histogram[i] )
+ break;
+ last = m_cost_histogram[i];
+ }
+ //std::cout << "J " << i << std::endl;
+
+ m_outlier_t2 = db_sqr(i*m_max_cost_pix/m_nr_bins);
+
+ //std::cout << "m_outlier_t2 " << m_outlier_t2 << std::endl;
+}
+
+void db_FrameToReferenceRegistration::SmoothMotion(void)
+{
+ VP_MOTION inmot,outmot;
+
+ double H[9];
+
+ Get_H_dref_to_ins(H);
+
+ MXX(inmot) = H[0];
+ MXY(inmot) = H[1];
+ MXZ(inmot) = H[2];
+ MXW(inmot) = 0.0;
+
+ MYX(inmot) = H[3];
+ MYY(inmot) = H[4];
+ MYZ(inmot) = H[5];
+ MYW(inmot) = 0.0;
+
+ MZX(inmot) = H[6];
+ MZY(inmot) = H[7];
+ MZZ(inmot) = H[8];
+ MZW(inmot) = 0.0;
+
+ MWX(inmot) = 0.0;
+ MWY(inmot) = 0.0;
+ MWZ(inmot) = 0.0;
+ MWW(inmot) = 1.0;
+
+ inmot.type = VP_MOTION_AFFINE;
+
+ int w = m_im_width;
+ int h = m_im_height;
+
+ if(m_quarter_resolution)
+ {
+ w = w*2;
+ h = h*2;
+ }
+
+#if 0
+ m_stab_smoother.smoothMotionAdaptive(w,h,&inmot,&outmot);
+#else
+ m_stab_smoother.smoothMotion(&inmot,&outmot);
+#endif
+
+ H[0] = MXX(outmot);
+ H[1] = MXY(outmot);
+ H[2] = MXZ(outmot);
+
+ H[3] = MYX(outmot);
+ H[4] = MYY(outmot);
+ H[5] = MYZ(outmot);
+
+ H[6] = MZX(outmot);
+ H[7] = MZY(outmot);
+ H[8] = MZZ(outmot);
+
+ Set_H_dref_to_ins(H);
+}
+
+void db_FrameToReferenceRegistration::GenerateQuarterResImage(const unsigned char* const* im)
+{
+ int input_h = m_im_height*2;
+ int input_w = m_im_width*2;
+
+ for (int j = 0; j < input_h; j++)
+ {
+ const unsigned char* in_row_ptr = im[j];
+ unsigned char* out_row_ptr = m_horz_smooth_subsample_image[j]+1;
+
+ for (int i = 2; i < input_w-2; i += 2)
+ {
+ int smooth_val = (
+ 6*in_row_ptr[i] +
+ ((in_row_ptr[i-1]+in_row_ptr[i+1])<<2) +
+ in_row_ptr[i-2]+in_row_ptr[i+2]
+ ) >> 4;
+ *out_row_ptr++ = (unsigned char) smooth_val;
+
+ if ( (smooth_val < 0) || (smooth_val > 255))
+ {
+ return;
+ //throw(std::exception());
+ }
+
+ }
+ }
+
+ for (int j = 2; j < input_h-2; j+=2)
+ {
+
+ unsigned char* in_row_ptr = m_horz_smooth_subsample_image[j];
+ unsigned char* out_row_ptr = m_quarter_res_image[j/2];
+
+ for (int i = 1; i < m_im_width-1; i++)
+ {
+ int smooth_val = (
+ 6*in_row_ptr[i] +
+ ((in_row_ptr[i-m_im_width]+in_row_ptr[i+m_im_width]) << 2)+
+ in_row_ptr[i-2*m_im_width]+in_row_ptr[i+2*m_im_width]
+ ) >> 4;
+ *out_row_ptr++ = (unsigned char)smooth_val;
+
+ if ( (smooth_val < 0) || (smooth_val > 255))
+ {
+ return;
+ //throw(std::exception());
+ }
+
+ }
+ }
+}