// reset easer to initial conditions, does not nuke easingFunc or arrivedFunc
void ServoEaser::reset()
{
currPos = servo.read();
startPos = currPos; // get everyone in sync
changePos = 0; // might be overwritten below
if( movesCount > 0 ) {
changePos = moves[ movesIndex ].pos - startPos ;
durMillis = moves[ movesIndex ].dur;
}
tickCount = durMillis / frameMillis;
tick = 0;
debug_reset();
}
//Processes a frame and returns output image
bool HiLight::processFrame(const cv::Mat& inputFrame, int* screen_position, double current_timestamp, char *results, bool* denoise_check, bool* first_bit_check, int* hilight_line_index, char* hilight_results_tmp )
{
*denoise_check = false;
*first_bit_check = false;
if(!initialized){
initialized = true;
for(int i = 0; i<grid_x; i++){
for(int j = 0; j<grid_y; j++ ){
for (int k = 0; k < first_bit_window; k++){
first_bit_color_window[i][j][k] = 0;
}
for (int k = 0; k < N; k++){
grid_color_intensity[i][j][k] = 0;
hilight_results_stack[i][j][k] = 0;
}
hilight_results[i][j] = 0;
}
}
if (isImage){
MVDR = true;
first_2_ratio = 0.6;//0.8
first_bit_voting = 0.5;//0.7
}else{
MVDR = false;
first_2_ratio = 0.8; //0.7
first_bit_voting = 0.7; //0.6
}
window_index = 0;
first_bit_detected = false;
first_bit_index = 0;
hilight_stack_index = 0;
start_time = current_timestamp;
current_time = current_timestamp;
denoise_start = false;
MVDR_index = 0;
bit_counter = 0;
results_stack_counter = 0;
fftSetup = vDSP_create_fftsetup(LOG_N, kFFTRadix2);
start_receiving = false;
counter_after_detect_1= 0;
first_bit_1_detected = false;
first_bit_2_detected = false;
hilight_first_bit_index = 0;
hilight_first_bit_counter = 0;
hilight_first_bit_position = 0;
if (screen_position[2] < screen_position[4]){
image_ROI_position[0].y = screen_position[2];
}else{
image_ROI_position[0].y = screen_position[4];
}
if (screen_position[1] < screen_position[7]){
image_ROI_position[0].x = screen_position[1];
}else{
image_ROI_position[0].x = screen_position[7];
}
if (screen_position[6] < screen_position[8]){
image_ROI_position[1].y = screen_position[8];
}else{
image_ROI_position[1].y = screen_position[6];
}
if (screen_position[5] < screen_position[3]){
image_ROI_position[1].x = screen_position[3];
}else{
image_ROI_position[1].x = screen_position[5];
}
if (isImage){
// Set grids points to calculate the grid position
for (int i = 0; i <= grid_x * grid_x_MVDR; i++){
grids_points_top_image[i].x = screen_position[1] + (screen_position[3] - screen_position[1]) / grid_x / grid_x_MVDR * i;
grids_points_top_image[i].y = screen_position[2] + (screen_position[4] - screen_position[2]) / grid_x / grid_x_MVDR * i;
grids_points_bottom_image[i].x = screen_position[7] + (screen_position[5] - screen_position[7]) / grid_x / grid_x_MVDR * i;
grids_points_bottom_image[i].y = screen_position[8] + (screen_position[6] - screen_position[8]) / grid_x / grid_x_MVDR * i;
}
for (int i = 0; i <= grid_y * grid_y_MVDR; i++){
grids_points_left_image[i].x = screen_position[1] + (screen_position[7] - screen_position[1]) / grid_y / grid_y_MVDR * i;
grids_points_left_image[i].y = screen_position[2] + (screen_position[8] - screen_position[2]) / grid_y / grid_y_MVDR * i;
grids_points_right_image[i].x = screen_position[3] + (screen_position[5] - screen_position[3]) / grid_y / grid_y_MVDR * i;
grids_points_right_image[i].y = screen_position[4] + (screen_position[6] - screen_position[4]) / grid_y / grid_y_MVDR * i;
}
for (int i = 0; i < grid_x * grid_x_MVDR; i++){
for (int j = 0; j < grid_y * grid_y_MVDR; j++){
cv::Point2f r1;
cv::Point2f r2;
cv::Point2f r3;
cv::Point2f r4;
intersection(grids_points_top_image[i], grids_points_bottom_image[i], grids_points_left_image[j], grids_points_right_image[j], r1);//top left
intersection(grids_points_top_image[i+1], grids_points_bottom_image[i+1], grids_points_left_image[j], grids_points_right_image[j], r2);//top right
intersection(grids_points_top_image[i+1], grids_points_bottom_image[i+1], grids_points_left_image[j+1], grids_points_right_image[j+1], r3);// bottom right
intersection(grids_points_top_image[i], grids_points_bottom_image[i], grids_points_left_image[j+1], grids_points_right_image[j+1], r4);//bottom left
//refine grid_position
if (r1.x <= r4.x){
grids_position_image[i][j][0].x = r4.x - image_ROI_position[0].x;
}else{
grids_position_image[i][j][0].x = r1.x - image_ROI_position[0].x;
}
if (r1.y <= r2.y){
grids_position_image[i][j][0].y = r2.y - image_ROI_position[0].y;
}else{
grids_position_image[i][j][0].y = r1.y - image_ROI_position[0].y;
}
if (r3.x <= r2.x){
grids_position_image[i][j][1].x = r3.x - image_ROI_position[0].x;
}else{
grids_position_image[i][j][1].x = r2.x - image_ROI_position[0].x;
}
if (r3.y <= r4.y){
grids_position_image[i][j][1].y = r3.y - image_ROI_position[0].y;
}else{
grids_position_image[i][j][1].y = r4.y - image_ROI_position[0].y;
}
}
}
}else{
// Set grids points to calculate the grid position
for (int i = 0; i <= grid_x; i++){
grids_points_top_video[i].x = screen_position[1] + (screen_position[3] - screen_position[1]) / grid_x * i;
grids_points_top_video[i].y = screen_position[2] + (screen_position[4] - screen_position[2]) / grid_x * i;
grids_points_bottom_video[i].x = screen_position[7] + (screen_position[5] - screen_position[7]) / grid_x * i;
grids_points_bottom_video[i].y = screen_position[8] + (screen_position[6] - screen_position[8]) / grid_x * i;
}
for (int i = 0; i <= grid_y; i++){
grids_points_left_video[i].x = screen_position[1] + (screen_position[7] - screen_position[1]) / grid_y * i;
grids_points_left_video[i].y = screen_position[2] + (screen_position[8] - screen_position[2]) / grid_y * i;
grids_points_right_video[i].x = screen_position[3] + (screen_position[5] - screen_position[3]) / grid_y * i;
grids_points_right_video[i].y = screen_position[4] + (screen_position[6] - screen_position[4]) / grid_y * i;
}
for (int i = 0; i < grid_x; i++){
for (int j = 0; j < grid_y; j++){
cv::Point2f r1;
cv::Point2f r2;
cv::Point2f r3;
cv::Point2f r4;
intersection(grids_points_top_video[i], grids_points_bottom_video[i], grids_points_left_video[j], grids_points_right_video[j], r1);//top left
intersection(grids_points_top_video[i+1], grids_points_bottom_video[i+1], grids_points_left_video[j], grids_points_right_video[j], r2);//top right
intersection(grids_points_top_video[i+1], grids_points_bottom_video[i+1], grids_points_left_video[j+1], grids_points_right_video[j+1], r3);// bottom right
intersection(grids_points_top_video[i], grids_points_bottom_video[i], grids_points_left_video[j+1], grids_points_right_video[j+1], r4);//bottom left
//refine grid_position
if (r1.x <= r4.x){
grids_position_video[i][j][0].x = r4.x - image_ROI_position[0].x;
}else{
grids_position_video[i][j][0].x = r1.x - image_ROI_position[0].x;
}
if (r1.y <= r2.y){
grids_position_video[i][j][0].y = r2.y - image_ROI_position[0].y;
}else{
grids_position_video[i][j][0].y = r1.y - image_ROI_position[0].y;
}
if (r3.x <= r2.x){
grids_position_video[i][j][1].x = r3.x - image_ROI_position[0].x;
}else{
grids_position_video[i][j][1].x = r2.x - image_ROI_position[0].x;
}
if (r3.y <= r4.y){
grids_position_video[i][j][1].y = r3.y - image_ROI_position[0].y;
}else{
grids_position_video[i][j][1].y = r4.y - image_ROI_position[0].y;
}
}
}
}
srcTri[0] = cv::Point2f( screen_position[2],screen_position[1] );
srcTri[1] = cv::Point2f( screen_position[4],screen_position[3] );
srcTri[2] = cv::Point2f( screen_position[6],screen_position[5] );
dist_top = sqrt(pow(screen_position[4]-screen_position[2],2.0) + pow(screen_position[3]-screen_position[1],2.0));
dstTri[0] = cv::Point2f( screen_position[2],screen_position[1] );
dstTri[1] = cv::Point2f( screen_position[2],screen_position[1] + dist_top );
dstTri[2] = cv::Point2f( screen_position[2] + (int)(dist_top*transmitter_screen_ratio),screen_position[1] + dist_top);
warp_mat = getAffineTransform( srcTri, dstTri );
}else{
current_time = current_timestamp;
}
//crop the transmitter screen from the captured image
cv::Mat image_ROI = inputFrame(cv::Range(image_ROI_position[0].y, image_ROI_position[1].y), cv::Range(image_ROI_position[0].x, image_ROI_position[1].x));
getGray_HiLight(image_ROI, grayImage);
image_ROI.release();
for (int c = 0; c < grid_x; c++){
for (int r = 0; r < grid_y; r++){
brightness[c][r] = 0;
}
}
if (isImage){
int brightness_c = 0;
int brightness_r = 0;
for (int c = 0; c < grid_x * grid_x_MVDR; c++){
for (int r = 0; r < grid_y * grid_y_MVDR; r++)
{
cv::Mat tile = grayImage(cv::Range(grids_position_image[c][r][0].y + grid_margin, grids_position_image[c][r][1].y - grid_margin)
, cv::Range(grids_position_image[c][r][0].x + grid_margin, grids_position_image[c][r][1].x - grid_margin));
cv::Scalar pixel_scalar = cv::mean(tile);
tile.release();
float brightness_tmp =pixel_scalar[0];
if (MVDR){
if(!denoise_start && MVDR_index < MVDR_frame){
grid_color_intensity_MVDR[c][r][MVDR_index] = brightness_tmp;
if ( c == grid_x * grid_x_MVDR -1 && r == grid_y * grid_y_MVDR - 1){
MVDR_index++;
}
}else if(!denoise_start && MVDR_index >= MVDR_frame){
denoise_start = true;
*denoise_check = true;
get_MVDR_weighting(grid_color_intensity_MVDR, MVDR_weighting);
//print MVDR matrix
if (isDebug){
for (int i = 0; i < grid_x * grid_x_MVDR; i++){
for (int j = 0; j < grid_y * grid_y_MVDR; j++){
printf("%f\t", MVDR_weighting[i][j]);
}
printf("\n");
}
}
}else{
brightness_c = floor(c*1.0/grid_x_MVDR);
brightness_r = floor(r*1.0/grid_y_MVDR);
brightness[brightness_c][brightness_r] = brightness[brightness_c][brightness_r] + brightness_tmp * MVDR_weighting[c][r];
}
}else{
brightness_c = floor(c*1.0/grid_x_MVDR);
brightness_r = floor(r*1.0/grid_y_MVDR);
brightness[brightness_c][brightness_r] = brightness[brightness_c][brightness_r] + brightness_tmp;
}
}
}
}else{
for (int c = 0; c < grid_x; c++){
for (int r = 0; r < grid_y; r++)
{
cv::Mat tile = grayImage(cv::Range(grids_position_video[c][r][0].y + grid_margin, grids_position_video[c][r][1].y - grid_margin)
, cv::Range(grids_position_video[c][r][0].x + grid_margin, grids_position_video[c][r][1].x - grid_margin));
cv::Scalar pixel_scalar = cv::mean(tile);
tile.release();
float brightness_tmp =pixel_scalar[0];
brightness[c][r] = brightness_tmp;
}
}
}
if (denoise_start || !MVDR){
for (int c = 0; c < grid_x; c++){
for (int r = 0; r < grid_y; r++){
if (window_index<(N-1)) {
grid_color_intensity[c][r][window_index] = brightness[c][r];
window_index++;
}else{
int i;
float fft_sum_N_over_2 = 0;
for (i = 1; i < N; i++){
grid_color_intensity[c][r][i-1] = grid_color_intensity[c][r][i];
}
grid_color_intensity[c][r][N-1] = brightness[c][r];
for (i = 0; i < N/2; i++){
fft_sum_N_over_2 = fft_sum_N_over_2 + grid_color_intensity[c][r][2*i] - grid_color_intensity[c][r][2*i+1];
}
// Initialize the input buffer with a sinusoid
// We need complex buffers in two different formats!
tempSplitComplex.realp = new float[N/2];
tempSplitComplex.imagp = new float[N/2];
// ----------------------------------------------------------------
// Forward FFT
// Scramble-pack the real data into complex buffer in just the way that's
// required by the real-to-complex FFT function that follows.
vDSP_ctoz((DSPComplex*)grid_color_intensity[c][r], 2, &tempSplitComplex, 1, N/2);
// Do real->complex forward FFT
vDSP_fft_zrip(fftSetup, &tempSplitComplex, 1, LOG_N, kFFTDirection_Forward);
int max_pulse_index = object_pulse_threashold;
float max_pulse = -100;
float object_pulse_power_1 = LinearToDecibel(pow (fft_sum_N_over_2, 2.0)/ N);
float object_pulse_power_2;
int object_pulse_force;
int object_pulse;
for (int k = object_pulse_threashold; k < N/2; k++)
{
float spectrum = LinearToDecibel((pow (tempSplitComplex.realp[k], 2.0) + pow (tempSplitComplex.imagp[k], 2.0))/ 4/ N);
if (max_pulse<spectrum){
max_pulse = spectrum;
max_pulse_index = k;
}
if(k == object_pulse_2){
object_pulse_power_2 = spectrum;
}
}
delete tempSplitComplex.realp;
delete tempSplitComplex.imagp;
if(max_pulse < object_pulse_power_1 && object_pulse_power_1 > -25){
object_pulse = 1;
}else if(max_pulse == object_pulse_power_2){
object_pulse = 2;
}else{
object_pulse = 0;
}
if(object_pulse_power_1 >= object_pulse_power_2){
object_pulse_force = 1;
}else{
object_pulse_force = 2;
}
//decode data by find the peak frequency power
if (first_bit_detected){
if (first_bit_1_detected && isCalibrate){
hilight_first_bit_stack[c][r][hilight_first_bit_index] = object_pulse_force;
}
if(current_time - start_time >= N / 60.0 * (bit_counter + 1)){
hilight_results[c][r] = get_hilight_results(hilight_results_stack[c][r], hilight_stack_index);
hilight_results_stack[c][r][0] = object_pulse_force;
}else{
hilight_results_stack[c][r][hilight_stack_index] = object_pulse_force;
}
}else{
first_bit_color_window[c][r][first_bit_index] = object_pulse;
}
}
}
}
if (first_bit_1_detected && isCalibrate){
hilight_first_bit_timestamp[hilight_first_bit_index++] = current_time;
}if(first_bit_2_detected && isCalibrate){
first_bit_2_detected = false;
hilight_first_bit_counter++;
if (hilight_first_bit_counter == N/2){
calibrate_first_bit_position();
}
}
if (first_bit_detected){
if(current_time - start_time >= N / 60.0 * (bit_counter + 1)){
int counter_for_2 = 0;
for (int i = 0; i < grid_x; i++){
for (int j = 0; j < grid_y; j++){
if (isDebug || start_receiving){
printf("%d ",hilight_results[i][j]);
}
if (hilight_results[i][j] == 2){
counter_for_2++;
}
}
}
if (isDebug || start_receiving){
printf("\n");
}
hilight_stack_index = 1;
bit_counter++;
if (!start_receiving){
counter_after_detect_1++;
if (counter_after_detect_1 > 3){
reset();
}else{
if(counter_for_2 >= (double)grid_x * grid_y * first_2_ratio){
first_bit_2_detected = true;
start_receiving = true;
*first_bit_check = true;
if (isDebug){
printf("\n");
}
}
}
return false;
}else{
int results_counter_tmp =0;
for (int i = 0; i < grid_x; i++){
for (int j = 0; j < grid_y; j++){
output_bit_stack[results_stack_counter++] = hilight_results[i][j];
hilight_results_tmp[results_counter_tmp++] = hilight_results[i][j] + '0' - 1;
}
}
hilight_line_index[0] = results_stack_counter/grid_x/grid_y;
if (results_stack_counter == output_bit_stck_length){
results_stack_counter = 0;
get_char_from_bits(output_bit_stack, results);
printf("%s\n",results);
if (demo){
debug_reset();
return true;
}else{
debug_reset();
return false;
}
}else{
return false;
}
}
}else{
hilight_stack_index++;
}
}else{
if(first_bit_index<first_bit_window-1){
first_bit_index++;
}else{
first_bit_detected = detect_first_bit(first_bit_color_window);
if (first_bit_detected){
if (isCalibrate){
first_bit_1_detected = true;
}
start_time = current_time;
}
}
}
}
return false;
}
