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