From 8872c23e739de38d74f04a8c852ebb5199c905f6 Mon Sep 17 00:00:00 2001 From: Michael Kolb Date: Tue, 29 Jan 2013 10:33:22 -0800 Subject: Move Camera Java/Native source into Gallery2 Change-Id: I968efe4d656e88a7760d3c0044f65b4adac2ddd1 --- jni_mosaic/feature_stab/src/dbreg/dbreg.cpp | 794 +++++++++++++++++++++ jni_mosaic/feature_stab/src/dbreg/dbreg.h | 581 +++++++++++++++ jni_mosaic/feature_stab/src/dbreg/dbstabsmooth.cpp | 330 +++++++++ jni_mosaic/feature_stab/src/dbreg/dbstabsmooth.h | 157 ++++ jni_mosaic/feature_stab/src/dbreg/targetver.h | 40 ++ jni_mosaic/feature_stab/src/dbreg/vp_motionmodel.c | 377 ++++++++++ jni_mosaic/feature_stab/src/dbreg/vp_motionmodel.h | 282 ++++++++ 7 files changed, 2561 insertions(+) create mode 100644 jni_mosaic/feature_stab/src/dbreg/dbreg.cpp create mode 100644 jni_mosaic/feature_stab/src/dbreg/dbreg.h create mode 100644 jni_mosaic/feature_stab/src/dbreg/dbstabsmooth.cpp create mode 100644 jni_mosaic/feature_stab/src/dbreg/dbstabsmooth.h create mode 100644 jni_mosaic/feature_stab/src/dbreg/targetver.h create mode 100644 jni_mosaic/feature_stab/src/dbreg/vp_motionmodel.c create mode 100644 jni_mosaic/feature_stab/src/dbreg/vp_motionmodel.h (limited to 'jni_mosaic/feature_stab/src/dbreg') diff --git a/jni_mosaic/feature_stab/src/dbreg/dbreg.cpp b/jni_mosaic/feature_stab/src/dbreg/dbreg.cpp new file mode 100644 index 000000000..da06aa2ab --- /dev/null +++ b/jni_mosaic/feature_stab/src/dbreg/dbreg.cpp @@ -0,0 +1,794 @@ +/* + * 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 +#include + + +#if PROFILE +#endif + +//#include + +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; km_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; k0 && 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 || force_reference) + { + 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(); + + // Disable debug printing + // 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 &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 &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 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; + } + + } + } + + 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; + } + + } + } +} diff --git a/jni_mosaic/feature_stab/src/dbreg/dbreg.h b/jni_mosaic/feature_stab/src/dbreg/dbreg.h new file mode 100644 index 000000000..4eb244481 --- /dev/null +++ b/jni_mosaic/feature_stab/src/dbreg/dbreg.h @@ -0,0 +1,581 @@ +/* + * 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. + */ + + +#pragma once + +#ifdef _WIN32 +#ifdef DBREG_EXPORTS +#define DBREG_API __declspec(dllexport) +#else +#define DBREG_API __declspec(dllimport) +#endif +#else +#define DBREG_API +#endif + +// @jke - the next few lines are for extracting timing data. TODO: Remove after test +#define PROFILE 0 + +#include "dbstabsmooth.h" + +#include +#include +#include + +#if PROFILE + #include +#endif + +/*! \mainpage db_FrameToReferenceRegistration + + \section intro Introduction + + db_FrameToReferenceRegistration provides a simple interface to a set of sophisticated algorithms for stabilizing + video sequences. As its name suggests, the class is used to compute parameters that will allow us to warp incoming video + frames and register them with respect to a so-called reference frame. The reference frame is simply the first + frame of a sequence; the registration process is that of estimating the parameters of a warp that can be applied to + subsequent frames to make those frames align with the reference. A video made up of these warped frames will be more + stable than the input video. + + For more technical information on the internal structure of the algorithms used within the db_FrameToRegistration class, + please follow this link. + + \section usage Usage + In addition to the class constructor, there are two main functions of db_FrameToReferenceRegistration that are of + interest to the programmer. db_FrameToReferenceRegistration::Init(...) is used to initialize the parameters of the + registration algorithm. db_FrameToReferenceRegistration::AddFrame(...) is the method by which each new video frame + is introduced to the registration algorithm, and produces the estimated registration warp parameters. + + The following example illustrates how the major methods of the class db_FrameToReferenceRegistration can be used together + to calculate the registration parameters for an image sequence. In the example, the calls to the methods of + db_FrameToReferenceRegistration match those found in the API, but supporting code should be considered pseudo-code. + For a more complete example, please consult the source code for dbregtest. + + + \code + // feature-based image registration class: + db_FrameToReferenceRegistration reg; + + // Image data + const unsigned char * const * image_storage; + + // The 3x3 frame to reference registration parameters + double frame_to_ref_homography[9]; + + // a counter to count the number of frames processed. + unsigned long frame_counter; + // ... + + // main loop - keep going while there are images to process. + while (ImagesAreAvailable) + { + // Call functions to place latest data into image_storage + // ... + + // if the registration object is not yet initialized, then do so + // The arguments to this function are explained in the accompanying + // html API documentation + if (!reg.Initialized()) + { + reg.Init(w,h,motion_model_type,25,linear_polish,quarter_resolution, + DB_POINT_STANDARDDEV,reference_update_period, + do_motion_smoothing,motion_smoothing_gain, + DB_DEFAULT_NR_SAMPLES,DB_DEFAULT_CHUNK_SIZE, + nr_corners,max_disparity); + } + + // Present the new image data to the registration algorithm, + // with the result being stored in the frame_to_ref_homography + // variable. + reg.AddFrame(image_storage,frame_to_ref_homography); + + // frame_to_ref_homography now contains the stabilizing transform + // use this to warp the latest image for display, etc. + + // if this is the first frame, we need to tell the registration + // class to store the image as its reference. Otherwise, AddFrame + // takes care of that. + if (frame_counter == 0) + { + reg.UpdateReference(image_storage); + } + + // increment the frame counter + frame_counter++; + } + + \endcode + + */ + +/*! + * Performs feature-based frame to reference image registration. + */ +class DBREG_API db_FrameToReferenceRegistration +{ +public: + db_FrameToReferenceRegistration(void); + ~db_FrameToReferenceRegistration(); + + /*! + * Set parameters and allocate memory. Note: The default values of these parameters have been set to the values used for the android implementation (i.e. the demo APK). + * \param width image width + * \param height image height + * \param homography_type see definitions in \ref LMRobImageHomography + * \param max_iterations max number of polishing steps + * \param linear_polish whether to perform a linear polishing step after RANSAC + * \param quarter_resolution whether to process input images at quarter resolution (for computational efficiency) + * \param scale Cauchy scale coefficient (see db_ExpCauchyReprojectionError() ) + * \param reference_update_period how often to update the alignment reference (in units of number of frames) + * \param do_motion_smoothing whether to perform display reference smoothing + * \param motion_smoothing_gain weight factor to reflect how fast the display reference must follow the current frame if motion smoothing is enabled + * \param nr_samples number of times to compute a hypothesis + * \param chunk_size size of cost chunks + * \param cd_target_nr_corners target number of corners for corner detector + * \param cm_max_disparity maximum disparity search range for corner matcher (in units of ratio of image width) + * \param cm_use_smaller_matching_window if set to true, uses a correlation window of 5x5 instead of the default 11x11 + * \param cd_nr_horz_blocks the number of horizontal blocks for the corner detector to partition the image + * \param cd_nr_vert_blocks the number of vertical blocks for the corner detector to partition the image + */ + void Init(int width, int height, + int homography_type = DB_HOMOGRAPHY_TYPE_DEFAULT, + int max_iterations = DB_DEFAULT_MAX_ITERATIONS, + bool linear_polish = false, + bool quarter_resolution = true, + double scale = DB_POINT_STANDARDDEV, + unsigned int reference_update_period = 3, + bool do_motion_smoothing = false, + double motion_smoothing_gain = 0.75, + int nr_samples = DB_DEFAULT_NR_SAMPLES, + int chunk_size = DB_DEFAULT_CHUNK_SIZE, + int cd_target_nr_corners = 500, + double cm_max_disparity = 0.2, + bool cm_use_smaller_matching_window = false, + int cd_nr_horz_blocks = 5, + int cd_nr_vert_blocks = 5); + + /*! + * Reset the transformation type that is being use to perform alignment. Use this to change the alignment type at run time. + * \param homography_type the type of transformation to use for performing alignment (see definitions in \ref LMRobImageHomography) + */ + void ResetHomographyType(int homography_type) { m_homography_type = homography_type; } + + /*! + * Enable/Disable motion smoothing. Use this to turn motion smoothing on/off at run time. + * \param enable flag indicating whether to turn the motion smoothing on or off. + */ + void ResetSmoothing(bool enable) { m_do_motion_smoothing = enable; } + + /*! + * Align an inspection image to an existing reference image, update the reference image if due and perform motion smoothing if enabled. + * \param im new inspection image + * \param H computed transformation from reference to inspection coordinate frame. Identity is returned if no reference frame was set. + * \param force_reference make this the new reference image + */ + int AddFrame(const unsigned char * const * im, double H[9], bool force_reference=false, bool prewarp=false); + + /*! + * Returns true if Init() was run. + */ + bool Initialized() const { return m_initialized; } + + /*! + * Returns true if the current frame is being used as the alignment reference. + */ + bool IsCurrentReference() const { return m_current_is_reference; } + + /*! + * Returns true if we need to call UpdateReference now. + */ + bool NeedReferenceUpdate(); + + /*! + * Returns the pointer reference to the alignment reference image data + */ + unsigned char ** GetReferenceImage() { return m_reference_image; } + + /*! + * Returns the pointer reference to the double array containing the homogeneous coordinates for the matched reference image corners. + */ + double * GetRefCorners() { return m_corners_ref; } + /*! + * Returns the pointer reference to the double array containing the homogeneous coordinates for the matched inspection image corners. + */ + double * GetInsCorners() { return m_corners_ins; } + /*! + * Returns the number of correspondences between the reference and inspection images. + */ + int GetNrMatches() { return m_nr_matches; } + + /*! + * Returns the number of corners detected in the current reference image. + */ + int GetNrRefCorners() { return m_nr_corners_ref; } + + /*! + * Returns the pointer to an array of indices that were found to be RANSAC inliers from the matched corner lists. + */ + int* GetInliers() { return m_inlier_indices; } + + /*! + * Returns the number of inliers from the RANSAC matching step. + */ + int GetNrInliers() { return m_num_inlier_indices; } + + //std::vector& GetInliers(); + //void Polish(std::vector &inlier_indices); + + /*! + * Perform a linear polishing step by re-estimating the alignment transformation using the RANSAC inliers. + * \param inlier_indices pointer to an array of indices that were found to be RANSAC inliers from the matched corner lists. + * \param num_inlier_indices number of inliers i.e. the length of the array passed as the first argument. + */ + void Polish(int *inlier_indices, int &num_inlier_indices); + + /*! + * Reset the motion smoothing parameters to their initial values. + */ + void ResetMotionSmoothingParameters() { m_stab_smoother.Init(); } + + /*! + * Update the alignment reference image to the specified image. + * \param im pointer to the image data to be used as the new alignment reference. + * \param subsample boolean flag to control whether the function should internally subsample the provided image to the size provided in the Init() function. + */ + int UpdateReference(const unsigned char * const * im, bool subsample = true, bool detect_corners = true); + + /*! + * Returns the transformation from the display reference to the alignment reference frame + */ + void Get_H_dref_to_ref(double H[9]); + /*! + * Returns the transformation from the display reference to the inspection reference frame + */ + void Get_H_dref_to_ins(double H[9]); + /*! + * Set the transformation from the display reference to the inspection reference frame + * \param H the transformation to set + */ + void Set_H_dref_to_ins(double H[9]); + + /*! + * Reset the display reference to the current frame. + */ + void ResetDisplayReference(); + + /*! + * Estimate a secondary motion model starting from the specified transformation. + * \param H the primary motion model to start from + */ + void EstimateSecondaryModel(double H[9]); + + /*! + * + */ + void SelectOutliers(); + + char *profile_string; + +protected: + void Clean(); + void GenerateQuarterResImage(const unsigned char* const * im); + + int m_im_width; + int m_im_height; + + // RANSAC and refinement parameters: + int m_homography_type; + int m_max_iterations; + double m_scale; + int m_nr_samples; + int m_chunk_size; + double m_outlier_t2; + + // Whether to fit a linear model to just the inliers at the end + bool m_linear_polish; + double m_polish_C[36]; + double m_polish_D[6]; + + // local state + bool m_current_is_reference; + bool m_initialized; + + // inspection to reference homography: + double m_H_ref_to_ins[9]; + double m_H_dref_to_ref[9]; + + // feature extraction and matching: + db_CornerDetector_u m_cd; + db_Matcher_u m_cm; + + // length of corner arrays: + unsigned long m_max_nr_corners; + + // corner locations of reference image features: + double * m_x_corners_ref; + double * m_y_corners_ref; + int m_nr_corners_ref; + + // corner locations of inspection image features: + double * m_x_corners_ins; + double * m_y_corners_ins; + int m_nr_corners_ins; + + // length of match index arrays: + unsigned long m_max_nr_matches; + + // match indices: + int * m_match_index_ref; + int * m_match_index_ins; + int m_nr_matches; + + // pointer to internal copy of the reference image: + unsigned char ** m_reference_image; + + // pointer to internal copy of last aligned inspection image: + unsigned char ** m_aligned_ins_image; + + // pointer to quarter resolution image, if used. + unsigned char** m_quarter_res_image; + + // temporary storage for the quarter resolution image processing + unsigned char** m_horz_smooth_subsample_image; + + // temporary space for homography computation: + double * m_temp_double; + int * m_temp_int; + + // homogenous image point arrays: + double * m_corners_ref; + double * m_corners_ins; + + // Indices of the points within the match lists + int * m_inlier_indices; + int m_num_inlier_indices; + + //void ComputeInliers(double H[9], std::vector &inlier_indices); + void ComputeInliers(double H[9]); + + // cost arrays: + void ComputeCostArray(); + bool m_sq_cost_computed; + double * m_sq_cost; + + // cost histogram: + void ComputeCostHistogram(); + int *m_cost_histogram; + + void SetOutlierThreshold(); + + // utility function for smoothing the motion parameters. + void SmoothMotion(void); + +private: + double m_K[9]; + const int m_over_allocation; + + bool m_reference_set; + + // Maximum number of inliers seen until now w.r.t the current reference frame + int m_max_inlier_count; + + // Number of cost histogram bins: + int m_nr_bins; + // All costs above this threshold get put into the last bin: + int m_max_cost_pix; + + // whether to quarter the image resolution for processing, or not + bool m_quarter_resolution; + + // the period (in number of frames) for reference update. + unsigned int m_reference_update_period; + + // the number of frames processed so far. + unsigned int m_nr_frames_processed; + + // smoother for motion transformations + db_StabilizationSmoother m_stab_smoother; + + // boolean to control whether motion smoothing occurs (or not) + bool m_do_motion_smoothing; + + // double to set the gain for motion smoothing + double m_motion_smoothing_gain; +}; +/*! + Create look-up tables to undistort images. Only Bougeut (Matlab toolkit) + is currently supported. Can be used with db_WarpImageLut_u(). + \code + xd = H*xs; + xd = xd/xd(3); + \endcode + \param lut_x pre-allocated float image + \param lut_y pre-allocated float image + \param w width + \param h height + \param H image homography from source to destination + */ +inline void db_GenerateHomographyLut(float ** lut_x,float ** lut_y,int w,int h,const double H[9]) +{ + assert(lut_x && lut_y); + double x[3] = {0.0,0.0,1.0}; + double xb[3]; + +/* + double xl[3]; + + // Determine the output coordinate system ROI + double Hinv[9]; + db_InvertAffineTransform(Hinv,H); + db_Multiply3x3_3x1(xl, Hinv, x); + xl[0] = db_SafeDivision(xl[0],xl[2]); + xl[1] = db_SafeDivision(xl[1],xl[2]); +*/ + + for ( int i = 0; i < w; ++i ) + for ( int j = 0; j < h; ++j ) + { + x[0] = double(i); + x[1] = double(j); + db_Multiply3x3_3x1(xb, H, x); + xb[0] = db_SafeDivision(xb[0],xb[2]); + xb[1] = db_SafeDivision(xb[1],xb[2]); + + lut_x[j][i] = float(xb[0]); + lut_y[j][i] = float(xb[1]); + } +} + +/*! + * Perform a look-up table warp for packed RGB ([rgbrgbrgb...]) images. + * The LUTs must be float images of the same size as source image. + * The source value x_s is determined from destination (x_d,y_d) through lut_x + * and y_s is determined from lut_y: + \code + x_s = lut_x[y_d][x_d]; + y_s = lut_y[y_d][x_d]; + \endcode + + * \param src source image (w*3 by h) + * \param dst destination image (w*3 by h) + * \param w width + * \param h height + * \param lut_x LUT for x + * \param lut_y LUT for y + */ +inline void db_WarpImageLutFast_rgb(const unsigned char * const * src, unsigned char ** dst, int w, int h, + const float * const * lut_x, const float * const * lut_y) +{ + assert(src && dst); + int xd=0, yd=0; + + for ( int i = 0; i < w; ++i ) + for ( int j = 0; j < h; ++j ) + { + xd = static_cast(lut_x[j][i]); + yd = static_cast(lut_y[j][i]); + if ( xd >= w || yd >= h || + xd < 0 || yd < 0) + { + dst[j][3*i ] = 0; + dst[j][3*i+1] = 0; + dst[j][3*i+2] = 0; + } + else + { + dst[j][3*i ] = src[yd][3*xd ]; + dst[j][3*i+1] = src[yd][3*xd+1]; + dst[j][3*i+2] = src[yd][3*xd+2]; + } + } +} + +inline unsigned char db_BilinearInterpolationRGB(double y, double x, const unsigned char * const * v, int offset) +{ + int floor_x=(int) x; + int floor_y=(int) y; + + int ceil_x=floor_x+1; + int ceil_y=floor_y+1; + + unsigned char f00 = v[floor_y][3*floor_x+offset]; + unsigned char f01 = v[floor_y][3*ceil_x+offset]; + unsigned char f10 = v[ceil_y][3*floor_x+offset]; + unsigned char f11 = v[ceil_y][3*ceil_x+offset]; + + double xl = x-floor_x; + double yl = y-floor_y; + + return (unsigned char)(f00*(1-yl)*(1-xl) + f10*yl*(1-xl) + f01*(1-yl)*xl + f11*yl*xl); +} + +inline void db_WarpImageLutBilinear_rgb(const unsigned char * const * src, unsigned char ** dst, int w, int h, + const float * const * lut_x, const float * const * lut_y) +{ + assert(src && dst); + double xd=0.0, yd=0.0; + + for ( int i = 0; i < w; ++i ) + for ( int j = 0; j < h; ++j ) + { + xd = static_cast(lut_x[j][i]); + yd = static_cast(lut_y[j][i]); + if ( xd > w-2 || yd > h-2 || + xd < 0.0 || yd < 0.0) + { + dst[j][3*i ] = 0; + dst[j][3*i+1] = 0; + dst[j][3*i+2] = 0; + } + else + { + dst[j][3*i ] = db_BilinearInterpolationRGB(yd,xd,src,0); + dst[j][3*i+1] = db_BilinearInterpolationRGB(yd,xd,src,1); + dst[j][3*i+2] = db_BilinearInterpolationRGB(yd,xd,src,2); + } + } +} + +inline double SquaredInhomogenousHomographyError(double y[3],double H[9],double x[3]){ + double x0,x1,x2,mult; + double sd; + + x0=H[0]*x[0]+H[1]*x[1]+H[2]; + x1=H[3]*x[0]+H[4]*x[1]+H[5]; + x2=H[6]*x[0]+H[7]*x[1]+H[8]; + mult=1.0/((x2!=0.0)?x2:1.0); + sd=(y[0]-x0*mult)*(y[0]-x0*mult)+(y[1]-x1*mult)*(y[1]-x1*mult); + + return(sd); +} + + +// functions related to profiling +#if PROFILE + +/* return current time in milliseconds */ +static double +now_ms(void) +{ + //struct timespec res; + struct timeval res; + //clock_gettime(CLOCK_REALTIME, &res); + gettimeofday(&res, NULL); + return 1000.0*res.tv_sec + (double)res.tv_usec/1e3; +} + +#endif diff --git a/jni_mosaic/feature_stab/src/dbreg/dbstabsmooth.cpp b/jni_mosaic/feature_stab/src/dbreg/dbstabsmooth.cpp new file mode 100644 index 000000000..dffff8ab1 --- /dev/null +++ b/jni_mosaic/feature_stab/src/dbreg/dbstabsmooth.cpp @@ -0,0 +1,330 @@ +/* + * 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. + */ + +#include +#include "dbstabsmooth.h" + +///// TODO TODO ////////// Replace this with the actual definition from Jayan's reply ///////////// +#define vp_copy_motion_no_id vp_copy_motion +/////////////////////////////////////////////////////////////////////////////////////////////////// + +static bool vpmotion_add(VP_MOTION *in1, VP_MOTION *in2, VP_MOTION *out); +static bool vpmotion_multiply(VP_MOTION *in1, double factor, VP_MOTION *out); + +db_StabilizationSmoother::db_StabilizationSmoother() +{ + Init(); +} + +void db_StabilizationSmoother::Init() +{ + f_smoothOn = true; + f_smoothReset = false; + f_smoothFactor = 1.0f; + f_minDampingFactor = 0.2f; + f_zoom = 1.0f; + VP_MOTION_ID(f_motLF); + VP_MOTION_ID(f_imotLF); + f_hsize = 0; + f_vsize = 0; + + VP_MOTION_ID(f_disp_mot); + VP_MOTION_ID(f_src_mot); + VP_MOTION_ID(f_diff_avg); + + for( int i = 0; i < MOTION_ARRAY-1; i++) { + VP_MOTION_ID(f_hist_mot_speed[i]); + VP_MOTION_ID(f_hist_mot[i]); + VP_MOTION_ID(f_hist_diff_mot[i]); + } + VP_MOTION_ID(f_hist_mot[MOTION_ARRAY-1]); + +} + +db_StabilizationSmoother::~db_StabilizationSmoother() +{} + + +bool db_StabilizationSmoother::smoothMotion(VP_MOTION *inmot, VP_MOTION *outmot) +{ + VP_MOTION_ID(f_motLF); + VP_MOTION_ID(f_imotLF); + f_motLF.insid = inmot->refid; + f_motLF.refid = inmot->insid; + + if(f_smoothOn) { + if(!f_smoothReset) { + MXX(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MXX(f_motLF) + (1.0-f_smoothFactor)* (double) MXX(*inmot)); + MXY(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MXY(f_motLF) + (1.0-f_smoothFactor)* (double) MXY(*inmot)); + MXZ(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MXZ(f_motLF) + (1.0-f_smoothFactor)* (double) MXZ(*inmot)); + MXW(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MXW(f_motLF) + (1.0-f_smoothFactor)* (double) MXW(*inmot)); + + MYX(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MYX(f_motLF) + (1.0-f_smoothFactor)* (double) MYX(*inmot)); + MYY(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MYY(f_motLF) + (1.0-f_smoothFactor)* (double) MYY(*inmot)); + MYZ(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MYZ(f_motLF) + (1.0-f_smoothFactor)* (double) MYZ(*inmot)); + MYW(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MYW(f_motLF) + (1.0-f_smoothFactor)* (double) MYW(*inmot)); + + MZX(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MZX(f_motLF) + (1.0-f_smoothFactor)* (double) MZX(*inmot)); + MZY(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MZY(f_motLF) + (1.0-f_smoothFactor)* (double) MZY(*inmot)); + MZZ(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MZZ(f_motLF) + (1.0-f_smoothFactor)* (double) MZZ(*inmot)); + MZW(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MZW(f_motLF) + (1.0-f_smoothFactor)* (double) MZW(*inmot)); + + MWX(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MWX(f_motLF) + (1.0-f_smoothFactor)* (double) MWX(*inmot)); + MWY(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MWY(f_motLF) + (1.0-f_smoothFactor)* (double) MWY(*inmot)); + MWZ(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MWZ(f_motLF) + (1.0-f_smoothFactor)* (double) MWZ(*inmot)); + MWW(f_motLF) = (VP_PAR) (f_smoothFactor*(double) MWW(f_motLF) + (1.0-f_smoothFactor)* (double) MWW(*inmot)); + } + else + vp_copy_motion_no_id(inmot, &f_motLF); // f_smoothFactor = 0.0 + + // Only allow LF motion to be compensated. Remove HF motion from + // the output transformation + if(!vp_invert_motion(&f_motLF, &f_imotLF)) + return false; + + if(!vp_cascade_motion(&f_imotLF, inmot, outmot)) + return false; + } + else { + vp_copy_motion_no_id(inmot, outmot); + } + + return true; +} + +bool db_StabilizationSmoother::smoothMotionAdaptive(/*VP_BIMG *bimg,*/int hsize, int vsize, VP_MOTION *inmot, VP_MOTION *outmot) +{ + VP_MOTION tmpMotion, testMotion; + VP_PAR p1x, p2x, p3x, p4x; + VP_PAR p1y, p2y, p3y, p4y; + double smoothFactor; + double minSmoothFactor = f_minDampingFactor; + +// int hsize = bimg->w; +// int vsize = bimg->h; + double border_factor = 0.01;//0.2; + double border_x = border_factor * hsize; + double border_y = border_factor * vsize; + + VP_MOTION_ID(f_motLF); + VP_MOTION_ID(f_imotLF); + VP_MOTION_ID(testMotion); + VP_MOTION_ID(tmpMotion); + + if (f_smoothOn) { + VP_MOTION identityMotion; + VP_MOTION_ID(identityMotion); // initialize the motion + vp_copy_motion(inmot/*in*/, &testMotion/*out*/); + VP_PAR delta = vp_motion_cornerdiff(&testMotion, &identityMotion, 0, 0,(int)hsize, (int)vsize); + + smoothFactor = 0.99 - 0.0015 * delta; + + if(smoothFactor < minSmoothFactor) + smoothFactor = minSmoothFactor; + + // Find the amount of motion that must be compensated so that no "border" pixels are seen in the stable video + for (smoothFactor = smoothFactor; smoothFactor >= minSmoothFactor; smoothFactor -= 0.01) { + // Compute the smoothed motion + if(!smoothMotion(inmot, &tmpMotion, smoothFactor)) + break; + + // TmpMotion, or Qsi where s is the smoothed display reference and i is the + // current image, tells us how points in the S co-ordinate system map to + // points in the I CS. We would like to check whether the four corners of the + // warped and smoothed display reference lies entirely within the I co-ordinate + // system. If yes, then the amount of smoothing is sufficient so that NO + // border pixels are seen at the output. We test for f_smoothFactor terms + // between 0.9 and 1.0, in steps of 0.01, and between 0.5 ands 0.9 in steps of 0.1 + + (void) vp_zoom_motion2d(&tmpMotion, &testMotion, 1, hsize, vsize, (double)f_zoom); // needs to return bool + + VP_WARP_POINT_2D(0, 0, testMotion, p1x, p1y); + VP_WARP_POINT_2D(hsize - 1, 0, testMotion, p2x, p2y); + VP_WARP_POINT_2D(hsize - 1, vsize - 1, testMotion, p3x, p3y); + VP_WARP_POINT_2D(0, vsize - 1, testMotion, p4x, p4y); + + if (!is_point_in_rect((double)p1x,(double)p1y,-border_x,-border_y,(double)(hsize+2.0*border_x),(double)(vsize+2.0*border_y))) { + continue; + } + if (!is_point_in_rect((double)p2x, (double)p2y,-border_x,-border_y,(double)(hsize+2.0*border_x),(double)(vsize+2.0*border_y))) { + continue; + } + if (!is_point_in_rect((double)p3x,(double)p3y,-border_x,-border_y,(double)(hsize+2.0*border_x),(double)(vsize+2.0*border_y))) { + continue; + } + if (!is_point_in_rect((double)p4x, (double)p4y,-border_x,-border_y,(double)(hsize+2.0*border_x),(double)(vsize+2.0*border_y))) { + continue; + } + + // If we get here, then all the points are in the rectangle. + // Therefore, break out of this loop + break; + } + + // if we get here and f_smoothFactor <= fMinDampingFactor, reset the stab reference + if (smoothFactor < f_minDampingFactor) + smoothFactor = f_minDampingFactor; + + // use the smoothed motion for stabilization + vp_copy_motion_no_id(&tmpMotion/*in*/, outmot/*out*/); + } + else + { + vp_copy_motion_no_id(inmot, outmot); + } + + return true; +} + +bool db_StabilizationSmoother::smoothMotion(VP_MOTION *inmot, VP_MOTION *outmot, double smooth_factor) +{ + f_motLF.insid = inmot->refid; + f_motLF.refid = inmot->insid; + + if(f_smoothOn) { + if(!f_smoothReset) { + MXX(f_motLF) = (VP_PAR) (smooth_factor*(double) MXX(f_motLF) + (1.0-smooth_factor)* (double) MXX(*inmot)); + MXY(f_motLF) = (VP_PAR) (smooth_factor*(double) MXY(f_motLF) + (1.0-smooth_factor)* (double) MXY(*inmot)); + MXZ(f_motLF) = (VP_PAR) (smooth_factor*(double) MXZ(f_motLF) + (1.0-smooth_factor)* (double) MXZ(*inmot)); + MXW(f_motLF) = (VP_PAR) (smooth_factor*(double) MXW(f_motLF) + (1.0-smooth_factor)* (double) MXW(*inmot)); + + MYX(f_motLF) = (VP_PAR) (smooth_factor*(double) MYX(f_motLF) + (1.0-smooth_factor)* (double) MYX(*inmot)); + MYY(f_motLF) = (VP_PAR) (smooth_factor*(double) MYY(f_motLF) + (1.0-smooth_factor)* (double) MYY(*inmot)); + MYZ(f_motLF) = (VP_PAR) (smooth_factor*(double) MYZ(f_motLF) + (1.0-smooth_factor)* (double) MYZ(*inmot)); + MYW(f_motLF) = (VP_PAR) (smooth_factor*(double) MYW(f_motLF) + (1.0-smooth_factor)* (double) MYW(*inmot)); + + MZX(f_motLF) = (VP_PAR) (smooth_factor*(double) MZX(f_motLF) + (1.0-smooth_factor)* (double) MZX(*inmot)); + MZY(f_motLF) = (VP_PAR) (smooth_factor*(double) MZY(f_motLF) + (1.0-smooth_factor)* (double) MZY(*inmot)); + MZZ(f_motLF) = (VP_PAR) (smooth_factor*(double) MZZ(f_motLF) + (1.0-smooth_factor)* (double) MZZ(*inmot)); + MZW(f_motLF) = (VP_PAR) (smooth_factor*(double) MZW(f_motLF) + (1.0-smooth_factor)* (double) MZW(*inmot)); + + MWX(f_motLF) = (VP_PAR) (smooth_factor*(double) MWX(f_motLF) + (1.0-smooth_factor)* (double) MWX(*inmot)); + MWY(f_motLF) = (VP_PAR) (smooth_factor*(double) MWY(f_motLF) + (1.0-smooth_factor)* (double) MWY(*inmot)); + MWZ(f_motLF) = (VP_PAR) (smooth_factor*(double) MWZ(f_motLF) + (1.0-smooth_factor)* (double) MWZ(*inmot)); + MWW(f_motLF) = (VP_PAR) (smooth_factor*(double) MWW(f_motLF) + (1.0-smooth_factor)* (double) MWW(*inmot)); + } + else + vp_copy_motion_no_id(inmot, &f_motLF); // smooth_factor = 0.0 + + // Only allow LF motion to be compensated. Remove HF motion from + // the output transformation + if(!vp_invert_motion(&f_motLF, &f_imotLF)) + return false; + + if(!vp_cascade_motion(&f_imotLF, inmot, outmot)) + return false; + } + else { + vp_copy_motion_no_id(inmot, outmot); + } + + return true; +} + +//! Overloaded smoother function that takes in user-specidied smoothing factor +bool +db_StabilizationSmoother::smoothMotion1(VP_MOTION *inmot, VP_MOTION *outmot, VP_MOTION *motLF, VP_MOTION *imotLF, double factor) +{ + + if(!f_smoothOn) { + vp_copy_motion(inmot, outmot); + return true; + } + else { + if(!f_smoothReset) { + MXX(*motLF) = (VP_PAR) (factor*(double) MXX(*motLF) + (1.0-factor)* (double) MXX(*inmot)); + MXY(*motLF) = (VP_PAR) (factor*(double) MXY(*motLF) + (1.0-factor)* (double) MXY(*inmot)); + MXZ(*motLF) = (VP_PAR) (factor*(double) MXZ(*motLF) + (1.0-factor)* (double) MXZ(*inmot)); + MXW(*motLF) = (VP_PAR) (factor*(double) MXW(*motLF) + (1.0-factor)* (double) MXW(*inmot)); + + MYX(*motLF) = (VP_PAR) (factor*(double) MYX(*motLF) + (1.0-factor)* (double) MYX(*inmot)); + MYY(*motLF) = (VP_PAR) (factor*(double) MYY(*motLF) + (1.0-factor)* (double) MYY(*inmot)); + MYZ(*motLF) = (VP_PAR) (factor*(double) MYZ(*motLF) + (1.0-factor)* (double) MYZ(*inmot)); + MYW(*motLF) = (VP_PAR) (factor*(double) MYW(*motLF) + (1.0-factor)* (double) MYW(*inmot)); + + MZX(*motLF) = (VP_PAR) (factor*(double) MZX(*motLF) + (1.0-factor)* (double) MZX(*inmot)); + MZY(*motLF) = (VP_PAR) (factor*(double) MZY(*motLF) + (1.0-factor)* (double) MZY(*inmot)); + MZZ(*motLF) = (VP_PAR) (factor*(double) MZZ(*motLF) + (1.0-factor)* (double) MZZ(*inmot)); + MZW(*motLF) = (VP_PAR) (factor*(double) MZW(*motLF) + (1.0-factor)* (double) MZW(*inmot)); + + MWX(*motLF) = (VP_PAR) (factor*(double) MWX(*motLF) + (1.0-factor)* (double) MWX(*inmot)); + MWY(*motLF) = (VP_PAR) (factor*(double) MWY(*motLF) + (1.0-factor)* (double) MWY(*inmot)); + MWZ(*motLF) = (VP_PAR) (factor*(double) MWZ(*motLF) + (1.0-factor)* (double) MWZ(*inmot)); + MWW(*motLF) = (VP_PAR) (factor*(double) MWW(*motLF) + (1.0-factor)* (double) MWW(*inmot)); + } + else { + vp_copy_motion(inmot, motLF); + } + // Only allow LF motion to be compensated. Remove HF motion from the output transformation + if(!vp_invert_motion(motLF, imotLF)) { +#if DEBUG_PRINT + printfOS("Invert failed \n"); +#endif + return false; + } + if(!vp_cascade_motion(imotLF, inmot, outmot)) { +#if DEBUG_PRINT + printfOS("cascade failed \n"); +#endif + return false; + } + } + return true; +} + + + + +bool db_StabilizationSmoother::is_point_in_rect(double px, double py, double rx, double ry, double w, double h) +{ + if (px < rx) + return(false); + if (px >= rx + w) + return(false); + if (py < ry) + return(false); + if (py >= ry + h) + return(false); + + return(true); +} + + + +static bool vpmotion_add(VP_MOTION *in1, VP_MOTION *in2, VP_MOTION *out) +{ + int i; + if(in1 == NULL || in2 == NULL || out == NULL) + return false; + + for(i = 0; i < VP_MAX_MOTION_PAR; i++) + out->par[i] = in1->par[i] + in2->par[i]; + + return true; +} + +static bool vpmotion_multiply(VP_MOTION *in1, double factor, VP_MOTION *out) +{ + int i; + if(in1 == NULL || out == NULL) + return false; + + for(i = 0; i < VP_MAX_MOTION_PAR; i++) + out->par[i] = in1->par[i] * factor; + + return true; +} + diff --git a/jni_mosaic/feature_stab/src/dbreg/dbstabsmooth.h b/jni_mosaic/feature_stab/src/dbreg/dbstabsmooth.h new file mode 100644 index 000000000..f03546ef6 --- /dev/null +++ b/jni_mosaic/feature_stab/src/dbreg/dbstabsmooth.h @@ -0,0 +1,157 @@ +/* + * 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. + */ + +#pragma once + + +#ifdef _WIN32 +#ifdef DBREG_EXPORTS +#define DBREG_API __declspec(dllexport) +#else +#define DBREG_API __declspec(dllimport) +#endif +#else +#define DBREG_API +#endif + +extern "C" { +#include "vp_motionmodel.h" +} + +#define MOTION_ARRAY 5 + + +/*! + * Performs smoothing on the motion estimate from feature_stab. + */ +class DBREG_API db_StabilizationSmoother +{ +public: + db_StabilizationSmoother(); + ~db_StabilizationSmoother(); + + /*! + * Initialize parameters for stab-smoother. + */ + void Init(); + + //! Smothing type + typedef enum { + SimpleSmooth = 0, //!< simple smooth + AdaptSmooth = 1, //!< adaptive smooth + PanSmooth = 2 //!< pan motion smooth + } SmoothType; + + /*! + * Smooth-motion is to do a weight-average between the current affine and + * motLF. The way to change the affine is only for the display purpose. + * It removes the high frequency motion and keep the low frequency motion + * to the display. IIR implmentation. + * \param inmot input motion parameters + * \param outmot smoothed output motion parameters + */ + bool smoothMotion(VP_MOTION *inmot, VP_MOTION *outmot); + + /*! + * The adaptive smoothing version of the above fixed smoothing function. + * \param hsize width of the image being aligned + * \param vsize height of the image being aligned + * \param inmot input motion parameters + * \param outmot smoothed output motion parameters + */ + bool smoothMotionAdaptive(/*VP_BIMG *bimg,*/int hsize, int vsize, VP_MOTION *inmot, VP_MOTION *outmot); + bool smoothPanMotion_1(VP_MOTION *inmot, VP_MOTION *outmot); + bool smoothPanMotion_2(VP_MOTION *inmot, VP_MOTION *outmot); + + /*! + * Set the smoothing factor for the stab-smoother. + * \param factor the factor value to set + */ + inline void setSmoothingFactor(float factor) { f_smoothFactor = factor; } + + /*! + * Reset smoothing + */ + inline void resetSmoothing(bool flag) { f_smoothReset = flag; } + /*! + * Set the zoom factor value. + * \param zoom the value to set to + */ + inline void setZoomFactor(float zoom) { f_zoom = zoom; } + /*! + * Set the minimum damping factor value. + * \param factor the value to set to + */ + inline void setminDampingFactor(float factor) { f_minDampingFactor = factor; } + + /*! + * Returns the current smoothing factor. + */ + inline float getSmoothingFactor(void) { return f_smoothFactor; } + /*! + * Returns the current zoom factor. + */ + inline float getZoomFactor(void) { return f_zoom; } + /*! + * Returns the current minimum damping factor. + */ + inline float getminDampingFactor(void) { return f_minDampingFactor; } + /*! + * Returns the current state of the smoothing reset flag. + */ + inline bool getSmoothReset(void) { return f_smoothReset; } + /*! + * Returns the current low frequency motion parameters. + */ + inline VP_MOTION getMotLF(void) { return f_motLF; } + /*! + * Returns the inverse of the current low frequency motion parameters. + */ + inline VP_MOTION getImotLF(void) { return f_imotLF; } + /*! + * Set the dimensions of the alignment image. + * \param hsize width of the image + * \param vsize height of the image + */ + inline void setSize(int hsize, int vsize) { f_hsize = hsize; f_vsize = vsize; } + +protected: + + bool smoothMotion(VP_MOTION *inmot, VP_MOTION *outmot, double smooth_factor); + bool smoothMotion1(VP_MOTION *inmot, VP_MOTION *outmot, VP_MOTION *motLF, VP_MOTION *imotLF, double smooth_factor); + void iterativeSmooth(VP_MOTION *input, VP_MOTION *output, double border_factor); + bool is_point_in_rect(double px, double py, double rx, double ry, double w, double h); + + +private: + int f_hsize; + int f_vsize; + bool f_smoothOn; + bool f_smoothReset; + float f_smoothFactor; + float f_minDampingFactor; + float f_zoom; + VP_MOTION f_motLF; + VP_MOTION f_imotLF; + VP_MOTION f_hist_mot[MOTION_ARRAY]; + VP_MOTION f_hist_mot_speed[MOTION_ARRAY-1]; + VP_MOTION f_hist_diff_mot[MOTION_ARRAY-1]; + VP_MOTION f_disp_mot; + VP_MOTION f_src_mot; + VP_MOTION f_diff_avg; + +}; + diff --git a/jni_mosaic/feature_stab/src/dbreg/targetver.h b/jni_mosaic/feature_stab/src/dbreg/targetver.h new file mode 100644 index 000000000..3ca3e8792 --- /dev/null +++ b/jni_mosaic/feature_stab/src/dbreg/targetver.h @@ -0,0 +1,40 @@ +/* + * 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. + */ + +#pragma once + +// The following macros define the minimum required platform. The minimum required platform +// is the earliest version of Windows, Internet Explorer etc. that has the necessary features to run +// your application. The macros work by enabling all features available on platform versions up to and +// including the version specified. + +// Modify the following defines if you have to target a platform prior to the ones specified below. +// Refer to MSDN for the latest info on corresponding values for different platforms. +#ifndef WINVER // Specifies that the minimum required platform is Windows Vista. +#define WINVER 0x0600 // Change this to the appropriate value to target other versions of Windows. +#endif + +#ifndef _WIN32_WINNT // Specifies that the minimum required platform is Windows Vista. +#define _WIN32_WINNT 0x0600 // Change this to the appropriate value to target other versions of Windows. +#endif + +#ifndef _WIN32_WINDOWS // Specifies that the minimum required platform is Windows 98. +#define _WIN32_WINDOWS 0x0410 // Change this to the appropriate value to target Windows Me or later. +#endif + +#ifndef _WIN32_IE // Specifies that the minimum required platform is Internet Explorer 7.0. +#define _WIN32_IE 0x0700 // Change this to the appropriate value to target other versions of IE. +#endif diff --git a/jni_mosaic/feature_stab/src/dbreg/vp_motionmodel.c b/jni_mosaic/feature_stab/src/dbreg/vp_motionmodel.c new file mode 100644 index 000000000..1f6af15bd --- /dev/null +++ b/jni_mosaic/feature_stab/src/dbreg/vp_motionmodel.c @@ -0,0 +1,377 @@ +/* + * 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. + */ + +/* +#sourcefile vpmotion/vp_motionmodel.c +#category motion-model +* +* Copyright 1998 Sarnoff Corporation +* All Rights Reserved +* +* Modification History +* Date: 02/14/98 +* Author: supuns +* Shop Order: 17xxx +* @(#) $Id: vp_motionmodel.c,v 1.4 2011/06/17 14:04:33 mbansal Exp $ +*/ + +/* +* =================================================================== +* Include Files +*/ + +#include /* memmove */ +#include +#include "vp_motionmodel.h" + +/* Static Functions */ +static +double Det3(double m[3][3]) +{ + double result; + + result = + m[0][0]*m[1][1]*m[2][2] + m[0][1]*m[1][2]*m[2][0] + + m[0][2]*m[1][0]*m[2][1] - m[0][2]*m[1][1]*m[2][0] - + m[0][0]*m[1][2]*m[2][1] - m[0][1]*m[1][0]*m[2][2]; + + return(result); +} + +typedef double MATRIX[4][4]; + +static +double Det4(MATRIX m) +{ + /* ==> This is a poor implementation of determinant. + Writing the formula out in closed form is unnecessarily complicated + and mistakes are easy to make. */ + double result; + + result= + m[0][3] *m[1][2] *m[2][1] *m[3][0] - m[0][2] *m[1][3] *m[2][1] *m[3][0] - m[0][3] *m[1][1] *m[2][2] *m[3][0] + + m[0][1] *m[1][3] *m[2][2] *m[3][0] + m[0][2] *m[1][1] *m[2][3] *m[3][0] - m[0][1] *m[1][2] *m[2][3] *m[3][0] - m[0][3] *m[1][2] *m[2][0] *m[3][1] + + m[0][2] *m[1][3] *m[2][0] *m[3][1] + m[0][3] *m[1][0] *m[2][2] *m[3][1] - m[0][0] *m[1][3] *m[2][2] *m[3][1] - m[0][2] *m[1][0] *m[2][3] *m[3][1] + + m[0][0] *m[1][2] *m[2][3] *m[3][1] + m[0][3] *m[1][1] *m[2][0] *m[3][2] - m[0][1] *m[1][3] *m[2][0] *m[3][2] - m[0][3] *m[1][0] *m[2][1] *m[3][2] + + m[0][0] *m[1][3] *m[2][1] *m[3][2] + m[0][1] *m[1][0] *m[2][3] *m[3][2] - m[0][0] *m[1][1] *m[2][3] *m[3][2] - m[0][2] *m[1][1] *m[2][0] *m[3][3] + + m[0][1] *m[1][2] *m[2][0] *m[3][3] + m[0][2] *m[1][0] *m[2][1] *m[3][3] - m[0][0] *m[1][2] *m[2][1] *m[3][3] - m[0][1] *m[1][0] *m[2][2] *m[3][3] + + m[0][0] *m[1][1] *m[2][2] *m[3][3]; + /* + m[0][0]*m[1][1]*m[2][2]*m[3][3]-m[0][1]*m[1][0]*m[2][2]*m[3][3]+ + m[0][1]*m[1][2]*m[2][0]*m[3][3]-m[0][2]*m[1][1]*m[2][0]*m[3][3]+ + m[0][2]*m[1][0]*m[2][1]*m[3][3]-m[0][0]*m[1][2]*m[2][1]*m[3][3]+ + m[0][0]*m[1][2]*m[2][3]*m[3][1]-m[0][2]*m[1][0]*m[2][3]*m[3][1]+ + m[0][2]*m[1][3]*m[2][0]*m[3][1]-m[0][3]*m[1][2]*m[2][0]*m[3][1]+ + m[0][3]*m[1][0]*m[2][2]*m[3][1]-m[0][0]*m[1][3]*m[2][2]*m[3][1]+ + m[0][0]*m[1][3]*m[2][1]*m[3][2]-m[0][3]*m[1][0]*m[2][3]*m[3][2]+ + m[0][1]*m[1][0]*m[2][3]*m[3][2]-m[0][0]*m[1][1]*m[2][0]*m[3][2]+ + m[0][3]*m[1][1]*m[2][0]*m[3][2]-m[0][1]*m[1][3]*m[2][1]*m[3][2]+ + m[0][1]*m[1][3]*m[2][2]*m[3][0]-m[0][3]*m[1][1]*m[2][2]*m[3][0]+ + m[0][2]*m[1][1]*m[2][3]*m[3][0]-m[0][1]*m[1][2]*m[2][3]*m[3][0]+ + m[0][3]*m[1][2]*m[2][1]*m[3][0]-m[0][2]*m[1][3]*m[2][1]*m[3][0]; + */ + return(result); +} + +static +int inv4Mat(const VP_MOTION* in, VP_MOTION* out) +{ + /* ==> This is a poor implementation of inversion. The determinant + method is O(N^4), i.e. unnecessarily slow, and not numerically accurate. + The real complexity of inversion is O(N^3), and is best done using + LU decomposition. */ + + MATRIX inmat,outmat; + int i, j, k, l, m, n,ntemp; + double mat[3][3], indet, temp; + + /* check for non-empty structures structure */ + if (((VP_MOTION *) NULL == in) || ((VP_MOTION *) NULL == out)) { + return 1; + } + + for(k=0,i=0;i<4;i++) + for(j=0;j<4;j++,k++) + inmat[i][j]=(double)in->par[k]; + + indet = Det4(inmat); + if (indet==0) return(-1); + + for (i=0;i<4;i++) { + for (j=0;j<4;j++) { + m = 0; + for (k=0;k<4;k++) { + if (i != k) { + n = 0; + for (l=0;l<4;l++) + if (j != l) { + mat[m][n] = inmat[k][l]; + n++; + } + m++; + } + } + + temp = -1.; + ntemp = (i +j ) %2; + if( ntemp == 0) temp = 1.; + + outmat[j][i] = temp * Det3(mat)/indet; + } + } + + for(k=0,i=0;i<4;i++) + for(j=0;j<4;j++,k++) + out->par[k]=(VP_PAR)outmat[i][j]; /*lint !e771*/ + + return(0); +} + +/* +* =================================================================== +* Public Functions +#htmlstart +*/ + +/* + * =================================================================== +#fn vp_invert_motion +#ft invert a motion +#fd DEFINITION + Bool + vp_invert_motion(const VP_MOTION* in,VP_MOTION* out) +#fd PURPOSE + This inverts the motion given in 'in'. + All motion models upto VP_MOTION_SEMI_PROJ_3D are supported. + It is assumed that the all 16 parameters are properly + initialized although you may not be using them. You could + use the VP_KEEP_ macro's defined in vp_motionmodel.h to set + the un-initialized parameters. This uses a 4x4 matrix invertion + function internally. + It is SAFE to pass the same pointer as both the 'in' and 'out' + parameters. +#fd INPUTS + in - input motion +#fd OUTPUTS + out - output inverted motion. If singular matrix uninitialized. + if MWW(in) is non-zero it is also normalized. +#fd RETURNS + FALSE - matrix is singular or motion model not supported + TRUE - otherwise +#fd SIDE EFFECTS + None +#endfn +*/ + +int vp_invert_motion(const VP_MOTION* in,VP_MOTION* out) +{ + int refid; + + /* check for non-empty structures structure */ + if (((VP_MOTION *) NULL == in) || ((VP_MOTION *) NULL == out)) { + return FALSE; + } + + if (in->type>VP_MOTION_SEMI_PROJ_3D) { + return FALSE; + } + + if (inv4Mat(in,out)<0) + return FALSE; + + /*VP_NORMALIZE(*out);*/ + out->type = in->type; + refid=in->refid; + out->refid=in->insid; + out->insid=refid; + return TRUE; +} + +/* +* =================================================================== +#fn vp_cascade_motion +#ft Cascade two motion transforms +#fd DEFINITION + Bool + vp_cascade_motion(const VP_MOTION* InAB,const VP_MOTION* InBC,VP_MOTION* OutAC) +#fd PURPOSE + Given Motion Transforms A->B and B->C, this function will + generate a New Motion that describes the transformation + from A->C. + More specifically, OutAC = InBC * InAC. + This function works ok if InAB,InBC and OutAC are the same pointer. +#fd INPUTS + InAB - First Motion Transform + InBC - Second Motion Tranform +#fd OUTPUTS + OutAC - Cascaded Motion +#fd RETURNS + FALSE - motion model not supported + TRUE - otherwise +#fd SIDE EFFECTS + None +#endfn +*/ + +int vp_cascade_motion(const VP_MOTION* InA, const VP_MOTION* InB,VP_MOTION* Out) +{ + /* ==> This is a poor implementation of matrix multiplication. + Writing the formula out in closed form is unnecessarily complicated + and mistakes are easy to make. */ + VP_PAR mxx,mxy,mxz,mxw; + VP_PAR myx,myy,myz,myw; + VP_PAR mzx,mzy,mzz,mzw; + VP_PAR mwx,mwy,mwz,mww; + + /* check for non-empty structures structure */ + if (((VP_MOTION *) NULL == InA) || ((VP_MOTION *) NULL == InB) || + ((VP_MOTION *) NULL == Out)) { + return FALSE; + } + + if (InA->type>VP_MOTION_PROJ_3D) { + return FALSE; + } + + if (InB->type>VP_MOTION_PROJ_3D) { + return FALSE; + } + + mxx = MXX(*InB)*MXX(*InA)+MXY(*InB)*MYX(*InA)+MXZ(*InB)*MZX(*InA)+MXW(*InB)*MWX(*InA); + mxy = MXX(*InB)*MXY(*InA)+MXY(*InB)*MYY(*InA)+MXZ(*InB)*MZY(*InA)+MXW(*InB)*MWY(*InA); + mxz = MXX(*InB)*MXZ(*InA)+MXY(*InB)*MYZ(*InA)+MXZ(*InB)*MZZ(*InA)+MXW(*InB)*MWZ(*InA); + mxw = MXX(*InB)*MXW(*InA)+MXY(*InB)*MYW(*InA)+MXZ(*InB)*MZW(*InA)+MXW(*InB)*MWW(*InA); + myx = MYX(*InB)*MXX(*InA)+MYY(*InB)*MYX(*InA)+MYZ(*InB)*MZX(*InA)+MYW(*InB)*MWX(*InA); + myy = MYX(*InB)*MXY(*InA)+MYY(*InB)*MYY(*InA)+MYZ(*InB)*MZY(*InA)+MYW(*InB)*MWY(*InA); + myz = MYX(*InB)*MXZ(*InA)+MYY(*InB)*MYZ(*InA)+MYZ(*InB)*MZZ(*InA)+MYW(*InB)*MWZ(*InA); + myw = MYX(*InB)*MXW(*InA)+MYY(*InB)*MYW(*InA)+MYZ(*InB)*MZW(*InA)+MYW(*InB)*MWW(*InA); + mzx = MZX(*InB)*MXX(*InA)+MZY(*InB)*MYX(*InA)+MZZ(*InB)*MZX(*InA)+MZW(*InB)*MWX(*InA); + mzy = MZX(*InB)*MXY(*InA)+MZY(*InB)*MYY(*InA)+MZZ(*InB)*MZY(*InA)+MZW(*InB)*MWY(*InA); + mzz = MZX(*InB)*MXZ(*InA)+MZY(*InB)*MYZ(*InA)+MZZ(*InB)*MZZ(*InA)+MZW(*InB)*MWZ(*InA); + mzw = MZX(*InB)*MXW(*InA)+MZY(*InB)*MYW(*InA)+MZZ(*InB)*MZW(*InA)+MZW(*InB)*MWW(*InA); + mwx = MWX(*InB)*MXX(*InA)+MWY(*InB)*MYX(*InA)+MWZ(*InB)*MZX(*InA)+MWW(*InB)*MWX(*InA); + mwy = MWX(*InB)*MXY(*InA)+MWY(*InB)*MYY(*InA)+MWZ(*InB)*MZY(*InA)+MWW(*InB)*MWY(*InA); + mwz = MWX(*InB)*MXZ(*InA)+MWY(*InB)*MYZ(*InA)+MWZ(*InB)*MZZ(*InA)+MWW(*InB)*MWZ(*InA); + mww = MWX(*InB)*MXW(*InA)+MWY(*InB)*MYW(*InA)+MWZ(*InB)*MZW(*InA)+MWW(*InB)*MWW(*InA); + + MXX(*Out)=mxx; MXY(*Out)=mxy; MXZ(*Out)=mxz; MXW(*Out)=mxw; + MYX(*Out)=myx; MYY(*Out)=myy; MYZ(*Out)=myz; MYW(*Out)=myw; + MZX(*Out)=mzx; MZY(*Out)=mzy; MZZ(*Out)=mzz; MZW(*Out)=mzw; + MWX(*Out)=mwx; MWY(*Out)=mwy; MWZ(*Out)=mwz; MWW(*Out)=mww; + /* VP_NORMALIZE(*Out); */ + Out->type= (InA->type > InB->type) ? InA->type : InB->type; + Out->refid=InA->refid; + Out->insid=InB->insid; + + return TRUE; +} + +/* +* =================================================================== +#fn vp_copy_motion +#ft Copies the source motion to the destination motion. +#fd DEFINITION + void + vp_copy_motion (const VP_MOTION *src, VP_MOTION *dst) +#fd PURPOSE + Copies the source motion to the destination motion. + It is OK if src == dst. + NOTE THAT THE SOURCE IS THE FIRST ARGUMENT. + This is different from some of the other VP + copy functions. +#fd INPUTS + src is the source motion + dst is the destination motion +#fd RETURNS + void +#endfn +*/ +void vp_copy_motion (const VP_MOTION *src, VP_MOTION *dst) +{ + /* Use memmove rather than memcpy because it handles overlapping memory + OK. */ + memmove(dst, src, sizeof(VP_MOTION)); + return; +} /* vp_copy_motion() */ + +#define VP_SQR(x) ( (x)*(x) ) +double vp_motion_cornerdiff(const VP_MOTION *mot_a, const VP_MOTION *mot_b, + int xo, int yo, int w, int h) +{ + double ax1, ay1, ax2, ay2, ax3, ay3, ax4, ay4; + double bx1, by1, bx2, by2, bx3, by3, bx4, by4; + double err; + + /*lint -e639 -e632 -e633 */ + VP_WARP_POINT_2D(xo, yo, *mot_a, ax1, ay1); + VP_WARP_POINT_2D(xo+w-1, yo, *mot_a, ax2, ay2); + VP_WARP_POINT_2D(xo+w-1, yo+h-1, *mot_a, ax3, ay3); + VP_WARP_POINT_2D(xo, yo+h-1, *mot_a, ax4, ay4); + VP_WARP_POINT_2D(xo, yo, *mot_b, bx1, by1); + VP_WARP_POINT_2D(xo+w-1, yo, *mot_b, bx2, by2); + VP_WARP_POINT_2D(xo+w-1, yo+h-1, *mot_b, bx3, by3); + VP_WARP_POINT_2D(xo, yo+h-1, *mot_b, bx4, by4); + /*lint +e639 +e632 +e633 */ + + err = 0; + err += (VP_SQR(ax1 - bx1) + VP_SQR(ay1 - by1)); + err += (VP_SQR(ax2 - bx2) + VP_SQR(ay2 - by2)); + err += (VP_SQR(ax3 - bx3) + VP_SQR(ay3 - by3)); + err += (VP_SQR(ax4 - bx4) + VP_SQR(ay4 - by4)); + + return(sqrt(err)); +} + +int vp_zoom_motion2d(VP_MOTION* in, VP_MOTION* out, + int n, int w, int h, double zoom) +{ + int ii; + VP_PAR inv_zoom; + VP_PAR cx, cy; + VP_MOTION R2r,R2f; + VP_MOTION *res; + + /* check for non-empty structures structure */ + if (((VP_MOTION *) NULL == in)||(zoom <= 0.0)||(w <= 0)||(h <= 0)) { + return FALSE; + } + + /* ==> Not sure why the special case of out=NULL is necessary. Why couldn't + the caller just pass the same pointer for both in and out? */ + res = ((VP_MOTION *) NULL == out)?in:out; + + cx = (VP_PAR) (w/2.0); + cy = (VP_PAR) (h/2.0); + + VP_MOTION_ID(R2r); + inv_zoom = (VP_PAR)(1.0/zoom); + MXX(R2r) = inv_zoom; + MYY(R2r) = inv_zoom; + MXW(R2r)=cx*(((VP_PAR)1.0) - inv_zoom); + MYW(R2r)=cy*(((VP_PAR)1.0) - inv_zoom); + + VP_KEEP_AFFINE_2D(R2r); + + for(ii=0;ii + +#define FALSE 0 +#define TRUE 1 + +#if 0 /* Moved mottomat.c and mattomot_d.c from vpmotion.h to vpcompat.h + in order to remove otherwise unnecessary dependency of vpmotion, + vpwarp, and newvpio on vpmath */ +#ifndef VPMATH_H +#include "vpmath.h" +#endif +#endif + +#if 0 +#ifndef VP_WARP_H +#include "vp_warp.h" +#endif +#endif +/* + +#htmlstart +# =================================================================== +#h 1 Introduction + + This defines a motion model that can describe translation, + affine, and projective projective 3d and 3d view transforms. + + The main structure VP_MOTION contains a 16 parameter array (That + can be considered as elements of a 4x4 matrix) and a type field + which can be one of VP_MOTION_NONE,VP_MOTION_TRANSLATION, + VP_MOTION_AFFINE, VP_MOTION_PROJECTIVE,VP_MOTION_PROJ_3D or + VP_MOTION_VIEW_3D. (These are defined using enums with gaps of 10 + so that subsets of these motions that are still consistant can be + added in between. Motion models that are inconsistant with this set + should be added at the end so the routines can hadle them + independently. + + The transformation VP_MOTION_NONE,VP_MOTION_TRANSLATION, + VP_MOTION_AFFINE, VP_MOTION_PROJECTIVE, VP_MOTION_PROJ_3D and + VP_MOTION_SEMI_PROJ_3D would map a point P={x,y,z,w} to a new point + P'={x',y',z',w'} using a motion model M such that P'= M.par * P. + Where M.par is thought of as elements of a 4x4 matrix ordered row + by row. The interpretation of all models except VP_MOTION_SEMI_PROJ_3D + is taken to be mapping of a 3d point P"={x",y",z"} which is obtained + from the normalization {x'/w',y'/w',z'/w'}. In the VP_MOTION_SEMI_PROJ_3D + the mapping to a point P"={x",y",z"} is obtained from the normalization + {x'/w',y'/w',z'}. All these motion models have the property that they + can be inverted using 4x4 matrices. Except for the VP_MOTION_SEMI_PROJ_3D all + other types can also be cascaded using 4x4 matrices. + + Specific macros and functions have been provided to handle 2d instances + of these functions. As the parameter interpretations can change when adding + new motion models it is HIGHLY RECOMMENDED that you use the macros MXX,MXY.. + ect. to interpret each motion component. +#pre +*/ + +/* +#endpre +# =================================================================== +#h 1 Typedef and Struct Declarations +#pre +*/ + +#define VP_MAX_MOTION_PAR 16 + +typedef double VP_PAR; +typedef VP_PAR VP_TRS[VP_MAX_MOTION_PAR]; + +/* Do not add any motion models before VP_MOTION_PROJECTIVE */ +/* The order is assumed in vp functions */ +enum VP_MOTION_MODEL { + VP_MOTION_NONE=0, + VP_MOTION_TRANSLATION=10, + VP_MOTION_SCALE=11, + VP_MOTION_ROTATE=12, + VP_MOTION_X_SHEAR=13, + VP_MOTION_Y_SHEAR=14, + VP_MOTION_SIMILARITY=15, + VP_MOTION_AFFINE=20, + VP_MOTION_PROJECTIVE=30, + VP_MOTION_PROJ_3D=40, + VP_MOTION_SEMI_PROJ_3D=80, + VP_SIMILARITY=100, + VP_VFE_AFFINE=120 +}; + +#define VP_REFID -1 /* Default ID used for reference frame */ + +typedef struct { + VP_TRS par; /* Contains the motion paramerers. + For the standard motion types this is + represented as 16 number that refer + to a 4x4 matrix */ + enum VP_MOTION_MODEL type; + int refid; /* Reference frame ( takes a point in refid frame + and moves it by the par to get a point in insid + frame ) */ + int insid; /* Inspection frame */ +} VP_MOTION; + +//typedef VP_LIST VP_MOTION_LIST; +/* +#endpre +# =================================================================== +#h 1 Constant Declarations +*/ + +/* Macros related to the 4x4 matrix parameters */ +#define MXX(m) (m).par[0] +#define MXY(m) (m).par[1] +#define MXZ(m) (m).par[2] +#define MXW(m) (m).par[3] +#define MYX(m) (m).par[4] +#define MYY(m) (m).par[5] +#define MYZ(m) (m).par[6] +#define MYW(m) (m).par[7] +#define MZX(m) (m).par[8] +#define MZY(m) (m).par[9] +#define MZZ(m) (m).par[10] +#define MZW(m) (m).par[11] +#define MWX(m) (m).par[12] +#define MWY(m) (m).par[13] +#define MWZ(m) (m).par[14] +#define MWW(m) (m).par[15] + +/* The do {...} while (0) technique creates a statement that can be used legally + in an if-else statement. See "Swallowing the semicolon", + http://gcc.gnu.org/onlinedocs/gcc-2.95.3/cpp_1.html#SEC23 */ +/* Initialize the Motion to be Identity */ +#define VP_MOTION_ID(m) do {\ + MXX(m)=MYY(m)=MZZ(m)=MWW(m)=(VP_PAR)1.0; \ + MXY(m)=MXZ(m)=MXW(m)=(VP_PAR)0.0; \ + MYX(m)=MYZ(m)=MYW(m)=(VP_PAR)0.0; \ + MZX(m)=MZY(m)=MZW(m)=(VP_PAR)0.0; \ + MWX(m)=MWY(m)=MWZ(m)=(VP_PAR)0.0; \ +(m).type = VP_MOTION_TRANSLATION; } while (0) + +/* Initialize without altering the translation components */ +#define VP_KEEP_TRANSLATION_3D(m) do {\ + MXX(m)=MYY(m)=MZZ(m)=MWW(m)=(VP_PAR)1.0; \ + MXY(m)=MXZ(m)=(VP_PAR)0.0; \ + MYX(m)=MYZ(m)=(VP_PAR)0.0; \ + MZX(m)=MZY(m)=(VP_PAR)0.0; \ + MWX(m)=MWY(m)=MWZ(m)=(VP_PAR)0.0; \ + (m).type = VP_MOTION_PROJ_3D; } while (0) + +/* Initialize without altering the 2d translation components */ +#define VP_KEEP_TRANSLATION_2D(m) do {\ + VP_KEEP_TRANSLATION_3D(m); MZW(m)=(VP_PAR)0.0; (m).type= VP_MOTION_TRANSLATION;} while (0) + +/* Initialize without altering the affine & translation components */ +#define VP_KEEP_AFFINE_3D(m) do {\ + MWX(m)=MWY(m)=MWZ(m)=(VP_PAR)0.0; MWW(m)=(VP_PAR)1.0; \ + (m).type = VP_MOTION_PROJ_3D; } while (0) + +/* Initialize without altering the 2d affine & translation components */ +#define VP_KEEP_AFFINE_2D(m) do {\ + VP_KEEP_AFFINE_3D(m); \ + MXZ(m)=MYZ(m)=(VP_PAR)0.0; MZZ(m)=(VP_PAR)1.0; \ + MZX(m)=MZY(m)=MZW(m)=(VP_PAR)0.0; \ + (m).type = VP_MOTION_AFFINE; } while (0) + +/* Initialize without altering the 2d projective parameters */ +#define VP_KEEP_PROJECTIVE_2D(m) do {\ + MXZ(m)=MYZ(m)=(VP_PAR)0.0; MZZ(m)=(VP_PAR)1.0; \ + MZX(m)=MZY(m)=MZW(m)=MWZ(m)=(VP_PAR)0.0; \ + (m).type = VP_MOTION_PROJECTIVE; } while (0) + +/* Warp a 2d point (assuming the z component is zero) */ +#define VP_WARP_POINT_2D(inx,iny,m,outx,outy) do {\ + VP_PAR vpTmpWarpPnt___= MWX(m)*(inx)+MWY(m)*(iny)+MWW(m); \ + outx = (MXX(m)*((VP_PAR)inx)+MXY(m)*((VP_PAR)iny)+MXW(m))/vpTmpWarpPnt___; \ + outy = (MYX(m)*((VP_PAR)inx)+MYY(m)*((VP_PAR)iny)+MYW(m))/vpTmpWarpPnt___; } while (0) + +/* Warp a 3d point */ +#define VP_WARP_POINT_3D(inx,iny,inz,m,outx,outy,outz) do {\ + VP_PAR vpTmpWarpPnt___= MWX(m)*(inx)+MWY(m)*(iny)+MWZ(m)*((VP_PAR)inz)+MWW(m); \ + outx = (MXX(m)*((VP_PAR)inx)+MXY(m)*((VP_PAR)iny)+MXZ(m)*((VP_PAR)inz)+MXW(m))/vpTmpWarpPnt___; \ + outy = (MYX(m)*((VP_PAR)inx)+MYY(m)*((VP_PAR)iny)+MYZ(m)*((VP_PAR)inz)+MYW(m))/vpTmpWarpPnt___; \ + outz = MZX(m)*((VP_PAR)inx)+MZY(m)*((VP_PAR)iny)+MZZ(m)*((VP_PAR)inz)+MZW(m); \ + if ((m).type==VP_MOTION_PROJ_3D) outz/=vpTmpWarpPnt___; } while (0) + +/* Projections of each component */ +#define VP_PROJW_3D(m,x,y,z,f) ( MWX(m)*(x)+MWY(m)*(y)+MWZ(m)*(z)+MWW(m) ) +#define VP_PROJX_3D(m,x,y,z,f,w) ((MXX(m)*(x)+MXY(m)*(y)+MXZ(m)*(z)+MXW(m))/(w)) +#define VP_PROJY_3D(m,x,y,z,f,w) ((MYX(m)*(x)+MYY(m)*(y)+MYZ(m)*(z)+MYW(m))/(w)) +#define VP_PROJZ_3D(m,x,y,z,f,w) ((MZX(m)*(x)+MZY(m)*(y)+MZZ(m)*(z)+MZW(m))/(w)) + +/* Scale Down a matrix by Sfactor */ +#define VP_SCALEDOWN(m,Sfactor) do { \ + MXW(m) /= (VP_PAR)Sfactor; MWX(m) *= (VP_PAR)Sfactor; \ + MYW(m) /= (VP_PAR)Sfactor; MWY(m) *= (VP_PAR)Sfactor; \ + MZW(m) /= (VP_PAR)Sfactor; MWZ(m) *= (VP_PAR)Sfactor; } while (0) + +/* Scale Up a matrix by Sfactor */ +#define VP_SCALEUP(m,Sfactor) do { \ + MXW(m) *= (VP_PAR)Sfactor; MWX(m) /= (VP_PAR)Sfactor; \ + MYW(m) *= (VP_PAR)Sfactor; MWY(m) /= (VP_PAR)Sfactor; \ + MZW(m) *= (VP_PAR)Sfactor; MWZ(m) /= (VP_PAR)Sfactor; } while (0) + +/* Normalize the transformation matrix so that MWW is 1 */ +#define VP_NORMALIZE(m) if (MWW(m)!=(VP_PAR)0.0) do { \ + MXX(m)/=MWW(m); MXY(m)/=MWW(m); MXZ(m)/=MWW(m); MXW(m)/= MWW(m); \ + MYX(m)/=MWW(m); MYY(m)/=MWW(m); MYZ(m)/=MWW(m); MYW(m)/= MWW(m); \ + MZX(m)/=MWW(m); MZY(m)/=MWW(m); MZZ(m)/=MWW(m); MZW(m)/= MWW(m); \ + MWX(m)/=MWW(m); MWY(m)/=MWW(m); MWZ(m)/=MWW(m); MWW(m) = (VP_PAR)1.0; } while (0) + +#define VP_PRINT_TRANS(msg,b) do { \ + fprintf(stderr, \ + "%s\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n", \ + msg, \ + MXX(b),MXY(b),MXZ(b),MXW(b), \ + MYX(b),MYY(b),MYZ(b),MYW(b), \ + MZX(b),MZY(b),MZZ(b),MZW(b), \ + MWX(b),MWY(b),MWZ(b),MWW(b)); \ +} while (0) + +/* w' projection given a point x,y,0,f */ +#define VP_PROJZ(m,x,y,f) ( \ + MWX(m)*((VP_PAR)x)+MWY(m)*((VP_PAR)y)+MWW(m)*((VP_PAR)f)) + +/* X Projection given a point x,y,0,f and w' */ +#define VP_PROJX(m,x,y,w,f) (\ + (MXX(m)*((VP_PAR)x)+MXY(m)*((VP_PAR)y)+MXW(m)*((VP_PAR)f))/((VP_PAR)w)) + +/* Y Projection given a point x,y,0,f and the w' */ +#define VP_PROJY(m,x,y,w,f) (\ + (MYX(m)*((VP_PAR)x)+MYY(m)*((VP_PAR)y)+MYW(m)*((VP_PAR)f))/((VP_PAR)w)) + +/* Set the reference id for a motion */ +#define VP_SET_REFID(m,id) do { (m).refid=id; } while (0) + +/* Set the inspection id for a motion */ +#define VP_SET_INSID(m,id) do { (m).insid=id; } while (0) + +void vp_copy_motion (const VP_MOTION *src, VP_MOTION *dst); +int vp_invert_motion(const VP_MOTION* in,VP_MOTION* out); +int vp_cascade_motion(const VP_MOTION* InAB, const VP_MOTION* InBC,VP_MOTION* OutAC); +int vp_zoom_motion2d(VP_MOTION* in, VP_MOTION* out, + int n, int w, int h, double zoom); +double vp_motion_cornerdiff(const VP_MOTION *mot_a, const VP_MOTION *mot_b, + int xo, int yo, int w, int h); + +#endif /* VP_MOTIONMODEL_H */ +/* =================================================================== */ +/* end vp_motionmodel.h */ -- cgit v1.2.3