From c84c5708e5e76deba94d0be3112574fbbbfe7efc Mon Sep 17 00:00:00 2001 From: dewagter Date: Wed, 7 Jan 2015 13:50:48 +0100 Subject: [PATCH] move computervision to modules --- .../OpticFlow/OpticFlowHover.xml | 44 + .../modules/computer_vision/OpticFlow/dummy.c | 3 + .../OpticFlow/hover_stabilization.c | 166 + .../OpticFlow/hover_stabilization.h | 46 + .../OpticFlow/opticflow_code.c | 332 + .../OpticFlow/opticflow_code.h | 49 + .../OpticFlow/opticflow_module.c | 218 + .../OpticFlow/opticflow_module.h | 45 + .../OpticFlow/video_message_structs.h | 32 + .../modules/computer_vision/cv/color.h | 6 +- .../computer_vision/cv/encoding/jpeg.c | 3 +- .../modules/computer_vision/cv/encoding/rtp.c | 16 +- .../cv/opticflow/fast9/LICENSE | 30 + .../cv/opticflow/fast9/fastRosten.c | 6069 +++++++++++++++++ .../cv/opticflow/fast9/fastRosten.h | 51 + .../cv/opticflow/optic_flow_ardrone.c | 624 ++ .../cv/opticflow/optic_flow_ardrone.h | 43 + .../modules/computer_vision/cv/resize.h | 6 +- sw/airborne/modules/computer_vision/cv/trig.c | 158 + sw/airborne/modules/computer_vision/cv/trig.h | 7 + .../modules/computer_vision/lib/paparazzi.h | 32 + .../modules/computer_vision/lib/tcp/socket.c | 139 + .../modules/computer_vision/lib/tcp/socket.h | 17 + .../modules/computer_vision/lib/udp/socket.c | 3 +- .../computer_vision/lib/v4l/ardrone2.c | 0 .../computer_vision/lib/v4l/ardrone2.h | 0 .../modules/computer_vision/lib/v4l/video.c | 49 +- .../modules/computer_vision/lib/v4l/video.h | 4 +- 28 files changed, 8143 insertions(+), 49 deletions(-) create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/OpticFlowHover.xml create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/dummy.c create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.c create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.h create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.c create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.h create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.c create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.h create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/video_message_structs.h create mode 100644 sw/airborne/modules/computer_vision/cv/opticflow/fast9/LICENSE create mode 100644 sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.c create mode 100644 sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.h create mode 100644 sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c create mode 100644 sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h create mode 100644 sw/airborne/modules/computer_vision/cv/trig.c create mode 100644 sw/airborne/modules/computer_vision/cv/trig.h create mode 100644 sw/airborne/modules/computer_vision/lib/paparazzi.h create mode 100644 sw/airborne/modules/computer_vision/lib/tcp/socket.c create mode 100644 sw/airborne/modules/computer_vision/lib/tcp/socket.h create mode 100644 sw/airborne/modules/computer_vision/lib/v4l/ardrone2.c create mode 100644 sw/airborne/modules/computer_vision/lib/v4l/ardrone2.h diff --git a/sw/airborne/modules/computer_vision/OpticFlow/OpticFlowHover.xml b/sw/airborne/modules/computer_vision/OpticFlow/OpticFlowHover.xml new file mode 100644 index 00000000000..3614379eb9a --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/OpticFlowHover.xml @@ -0,0 +1,44 @@ + + + + + Video ARDone 2 + + +
+ +
+ + + + + + + + + +include $(PAPARAZZI_HOME)/sw/ext/ardrone2_vision/Makefile.paths + +VISION_MODULE_FOLDER = $(DIR_MODULES)/OpticFlow + +$(TARGET).CFLAGS += -I$(DIR_MODULES) -I$(DIR_CV) -I$(DIR_LIB) -pthread -D__USE_GNU +$(TARGET).CXXFLAGS += -I$(PAPARAZZI_HOME)/sw/include/ -I$(PAPARAZZI_SRC)/sw/airborne -I$(PAPARAZZI_SRC)/conf/autopilot -I$(PAPARAZZI_SRC)/sw/airborne/arch/$($(TARGET).ARCHDIR) -I$(VARINCLUDE) -I$(ACINCLUDE) -I$(PAPARAZZI_SRC)/sw/airborne/modules/ +$(TARGET).CXXFLAGS += -I$(DIR_MODULES) -I$(DIR_CV) -I$(DIR_LIB) -pthread -D__USE_GNU + +$(TARGET).srcs += $(VISION_MODULE_FOLDER)/opticflow_module.c +$(TARGET).srcs += $(VISION_MODULE_FOLDER)/opticflow_code.c +$(TARGET).srcs += $(VISION_MODULE_FOLDER)/hover_stabilization.c +$(TARGET).srcs += $(DIR_CV)/opticflow/optic_flow_ardrone.c +$(TARGET).srcs += $(DIR_CV)/opticflow/fast9/fastRosten.c +$(TARGET).srcs += $(DIR_CV)/encoding/jpeg.c +$(TARGET).srcs += $(DIR_CV)/encoding/rtp.c +$(TARGET).srcs += $(DIR_CV)/trig.c +$(TARGET).srcs += $(DIR_LIB)/udp/socket.c +$(TARGET).srcs += $(DIR_LIB)/v4l/video.c +$(TARGET).CFLAGS += -I$(DIR_MODULES) -I$(DIR_CV) -I$(DIR_LIB) -pthread +$(TARGET).LDFLAGS += -pthread -lrt -static + + + + + diff --git a/sw/airborne/modules/computer_vision/OpticFlow/dummy.c b/sw/airborne/modules/computer_vision/OpticFlow/dummy.c new file mode 100644 index 00000000000..63978a0e13c --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/dummy.c @@ -0,0 +1,3 @@ +void dummyFunction(void) { + return; +} diff --git a/sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.c b/sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.c new file mode 100644 index 00000000000..0acb8b76745 --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.c @@ -0,0 +1,166 @@ +/* + * Copyright (C) 2014 Hann Woei Ho + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/hover_stabilization.c + * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * + * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 + */ + +// Own Header +#include "hover_stabilization.h" + +// Vision Data +#include "opticflow_code.h" + +// Stabilization +//#include "stabilization.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h" +#include "autopilot.h" + +// Downlink +#include "subsystems/datalink/downlink.h" + +// Controller Gains +/* error if some gains are negative */ +#if (VISION_PHI_PGAIN < 0) || \ + (VISION_PHI_IGAIN < 0) || \ + (VISION_THETA_PGAIN < 0) || \ + (VISION_THETA_IGAIN < 0) +#error "ALL control gains have to be positive!!!" +#endif +bool activate_opticflow_hover; +float vision_desired_vx; +float vision_desired_vy; +int32_t vision_phi_pgain; +int32_t vision_phi_igain; +int32_t vision_theta_pgain; +int32_t vision_theta_igain; + +// Controller Commands +struct Int32Eulers cmd_euler; + +// Hover Stabilization +float Velx_Int; +float Vely_Int; +float Error_Velx; +float Error_Vely; + +#define CMD_OF_SAT 1500 // 40 deg = 2859.1851 +unsigned char saturateX = 0, saturateY = 0; +unsigned int set_heading; + +void init_hover_stabilization_onvision() +{ + INT_EULERS_ZERO(cmd_euler); + + activate_opticflow_hover = VISION_HOVER; + vision_phi_pgain = VISION_PHI_PGAIN; + vision_phi_igain = VISION_PHI_IGAIN; + vision_theta_pgain = VISION_THETA_PGAIN; + vision_theta_igain = VISION_THETA_IGAIN; + vision_desired_vx = VISION_DESIRED_VX; + vision_desired_vy = VISION_DESIRED_VY; + + set_heading = 1; + + Error_Velx = 0; + Error_Vely = 0; + Velx_Int = 0; + Vely_Int = 0; +} + +void run_hover_stabilization_onvision(void) +{ + if(autopilot_mode == AP_MODE_VISION_HOVER) + { + run_opticflow_hover(); + } + else + { + Velx_Int = 0; + Vely_Int = 0; + } +} + +void run_opticflow_hover(void) +{ + if(flow_count) + { + Error_Velx = Velx - vision_desired_vx; + Error_Vely = Vely - vision_desired_vy; + } + else + { + Error_Velx = 0; + Error_Vely = 0; + } + + if(saturateX==0) + { + if(activate_opticflow_hover==TRUE) + { + Velx_Int += vision_theta_igain*Error_Velx; + } + else + { + Velx_Int += vision_theta_igain*V_body.x; + } + } + if(saturateY==0) + { + if(activate_opticflow_hover==TRUE) + { + Vely_Int += vision_phi_igain*Error_Vely; + } + else + { + Vely_Int += vision_phi_igain*V_body.y; + } + } + + if(set_heading) + { + cmd_euler.psi = stateGetNedToBodyEulers_i()->psi; + set_heading = 0; + } + + if(activate_opticflow_hover==TRUE) + { + cmd_euler.phi = - (vision_phi_pgain*Error_Vely + Vely_Int); + cmd_euler.theta = (vision_theta_pgain*Error_Velx + Velx_Int); + } + else + { + cmd_euler.phi = - (vision_phi_pgain*V_body.y + Vely_Int); + cmd_euler.theta = (vision_theta_pgain*V_body.x + Velx_Int); + } + + saturateX = 0; saturateY = 0; + if(cmd_euler.phi<-CMD_OF_SAT){cmd_euler.phi = -CMD_OF_SAT; saturateX = 1;} + else if(cmd_euler.phi>CMD_OF_SAT){cmd_euler.phi = CMD_OF_SAT; saturateX = 1;} + if(cmd_euler.theta<-CMD_OF_SAT){cmd_euler.theta = -CMD_OF_SAT; saturateY = 1;} + else if(cmd_euler.theta>CMD_OF_SAT){cmd_euler.theta = CMD_OF_SAT;saturateY = 1;} + + stabilization_attitude_set_rpy_setpoint_i(&cmd_euler); + DOWNLINK_SEND_VISION_STABILIZATION(DefaultChannel, DefaultDevice, &Velx, &Vely, &Velx_Int, &Vely_Int, &cmd_euler.phi, &cmd_euler.theta); +} diff --git a/sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.h b/sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.h new file mode 100644 index 00000000000..a0a69dda9cc --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.h @@ -0,0 +1,46 @@ +/* + * Copyright (C) 2014 Hann Woei Ho + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/hover_stabilization.h + * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * + * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 + */ + +#ifndef HOVER_STABILIZATION_H_ +#define HOVER_STABILIZATION_H_ + +#include + +void init_hover_stabilization_onvision(void); +void run_hover_stabilization_onvision(void); +void run_opticflow_hover(void); + +extern bool activate_opticflow_hover; +extern float vision_desired_vx; +extern float vision_desired_vy; +extern int32_t vision_phi_pgain; +extern int32_t vision_phi_igain; +extern int32_t vision_theta_pgain; +extern int32_t vision_theta_igain; + +#endif /* HOVER_STABILIZATION_H_ */ diff --git a/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.c b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.c new file mode 100644 index 00000000000..bac9bfc3382 --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.c @@ -0,0 +1,332 @@ +/* + * Copyright (C) 2014 Hann Woei Ho + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_code.c + * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * + * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 + */ + +#include +#include + +// Own Header +#include "opticflow_code.h" + +// Computer Vision +#include "opticflow/optic_flow_gdc.h" +#include "opticflow/fast9/fastRosten.h" +#include "opticflow_module.h" + +// Paparazzi Data +#include "subsystems/ins/ins_int.h" +#include "subsystems/imu.h" + +// Downlink +#include "subsystems/datalink/downlink.h" + +// Timer +#include + +// Settable by plugin +unsigned int imgWidth, imgHeight; +unsigned int verbose = 0; + +// Local variables +unsigned char *prev_frame, *gray_frame, *prev_gray_frame; +int old_img_init; +float OFx, OFy, dx_sum, dy_sum; + +// ARDrone Vertical Camera Parameters +#define FOV_H 0.67020643276 +#define FOV_W 0.89360857702 +#define Fx_ARdrone 343.1211 +#define Fy_ARdrone 348.5053 + +// Corner Detection +int *x, *y; +int count = 0; +int max_count = 25; +#define MAX_COUNT 100 + +// Corner Tracking +int *new_x, *new_y, *status, *dx, *dy; +int error_opticflow; +int flow_count = 0; +int remove_point; +int c; +int borderx = 24, bordery = 24; + +// Remove bad corners +float distance2, min_distance, min_distance2; + +// Flow Derotation +#define FLOW_DEROTATION +float curr_pitch, curr_roll, curr_yaw, prev_pitch, prev_roll; +float cam_h, diff_roll, diff_pitch, OFx_trans, OFy_trans; + +// Lateral Velocity Computation +float Velx, Vely; + +// Compute body velocities +struct FloatVect3 V_Ned; +struct FloatRMat Rmat_Ned2Body; +struct FloatVect3 V_body; + +// Called by plugin +void my_plugin_init(void) +{ + // Initialize variables + gray_frame = (unsigned char *) calloc(imgWidth*imgHeight,sizeof(unsigned char)); + prev_frame = (unsigned char *) calloc(imgWidth*imgHeight*2,sizeof(unsigned char)); + prev_gray_frame = (unsigned char *) calloc(imgWidth*imgHeight,sizeof(unsigned char)); + x = (int *) calloc(MAX_COUNT,sizeof(int)); + new_x = (int *) calloc(MAX_COUNT,sizeof(int)); + y = (int *) calloc(MAX_COUNT,sizeof(int)); + new_y = (int *) calloc(MAX_COUNT,sizeof(int)); + status = (int *) calloc(MAX_COUNT,sizeof(int)); + dx = (int *) calloc(MAX_COUNT,sizeof(int)); + dy = (int *) calloc(MAX_COUNT,sizeof(int)); + old_img_init = 1; + OFx = 0.0; + OFy = 0.0; + dx_sum = 0.0; + dy_sum = 0.0; + diff_roll = 0.0; + diff_pitch = 0.0; + cam_h = 0.0; + prev_pitch = 0.0; + prev_roll = 0.0; + curr_pitch = 0.0; + curr_roll = 0.0; + curr_yaw = 0.0; + OFx_trans = 0.0; + OFy_trans = 0.0; + Velx = 0.0; + Vely = 0.0; +} + +void my_plugin_run(unsigned char *frame) +{ + if(old_img_init == 1) + { + memcpy(prev_frame,frame,imgHeight*imgWidth*2); + CvtYUYV2Gray(prev_gray_frame, prev_frame, imgWidth, imgHeight); + old_img_init = 0; + } + + // *********************************************************************************************************************** + // Additional information from other sensors + // *********************************************************************************************************************** + + // Compute body velocities from ENU + V_Ned.x = stateGetSpeedNed_f()->x; + V_Ned.y = stateGetSpeedNed_f()->y; + V_Ned.z = stateGetSpeedNed_f()->z; + + struct FloatQuat* BodyQuaternions = stateGetNedToBodyQuat_f(); + FLOAT_RMAT_OF_QUAT(Rmat_Ned2Body,*BodyQuaternions); + RMAT_VECT3_MUL(V_body, Rmat_Ned2Body, V_Ned); + + // *********************************************************************************************************************** + // Corner detection + // *********************************************************************************************************************** + + // FAST corner detection + int fast_threshold = 20; + xyFAST* pnts_fast; + pnts_fast = fast9_detect((const byte*)prev_gray_frame, imgWidth, imgHeight, imgWidth, fast_threshold, &count); + + if(count > MAX_COUNT) count = MAX_COUNT; + for(int i = 0; i < count; i++) + { + x[i] = pnts_fast[i].x; + y[i] = pnts_fast[i].y; + } + free(pnts_fast); + + // Remove neighbouring corners + min_distance = 3; + min_distance2 = min_distance*min_distance; + int *labelmin; + labelmin = (int *) calloc(MAX_COUNT,sizeof(int)); + for(int i = 0; i < count; i++) + { + for(int j = i+1; j < count; j++) + { + // distance squared: + distance2 = (x[i] - x[j])*(x[i] - x[j]) + (y[i] - y[j])*(y[i] - y[j]); + if(distance2 < min_distance2) + { + labelmin[i] = 1; + } + } + } + + int count_fil = count; + for(int i = count-1; i >= 0; i-- ) + { + remove_point = 0; + + if(labelmin[i]) + { + remove_point = 1; + } + + if(remove_point) + { + for(c = i; c max_count) count_fil = max_count; + count = count_fil; + free(labelmin); + + // ********************************************************************************************************************** + // Corner Tracking + // ********************************************************************************************************************** + CvtYUYV2Gray(gray_frame, frame, imgWidth, imgHeight); + + error_opticflow = opticFlowLK(gray_frame, prev_gray_frame, x, y, count_fil, imgWidth, imgHeight, new_x, new_y, status, 5, 100); + + flow_count = count_fil; + for(int i=count_fil-1; i>=0; i--) + { + remove_point = 1; + + if(status[i] && !(new_x[i] < borderx || new_x[i] > (imgWidth-1-borderx) || + new_y[i] < bordery || new_y[i] > (imgHeight-1-bordery))) + { + remove_point = 0; + } + + if(remove_point) + { + for(c = i; c theta; + curr_roll = stateGetNedToBodyEulers_f()->phi; + curr_yaw = stateGetNedToBodyEulers_f()->psi; + + diff_pitch = (curr_pitch - prev_pitch)*imgHeight/FOV_H; + diff_roll = (curr_roll - prev_roll)*imgWidth/FOV_W; + + prev_pitch = curr_pitch; + prev_roll = curr_roll; + +#ifdef FLOW_DEROTATION + if(flow_count) + { + OFx_trans = dx_sum - diff_roll; + OFy_trans = dy_sum - diff_pitch; + + if((OFx_trans<=0) != (dx_sum<=0)) + { + OFx_trans = 0; + OFy_trans = 0; + } + } + else + { + OFx_trans = dx_sum; + OFy_trans = dy_sum; + } +#else + OFx_trans = dx_sum; + OFy_trans = dy_sum; +#endif + + // Average Filter + OFfilter(&OFx, &OFy, OFx_trans, OFy_trans, flow_count, 1); + + // Velocity Computation + #ifdef USE_SONAR + cam_h = ins_impl.sonar_z; + #else + cam_h = 1; + #endif + + if(flow_count) + { + Velx = OFy*cam_h*FPS/Fy_ARdrone + 0.05; + Vely = -OFx*cam_h*FPS/Fx_ARdrone - 0.1; + } + else + { + Velx = 0.0; + Vely = 0.0; + } + + // ********************************************************************************************************************** + // Next Loop Preparation + // ********************************************************************************************************************** + + memcpy(prev_frame,frame,imgHeight*imgWidth*2); + memcpy(prev_gray_frame,gray_frame,imgHeight*imgWidth); + + // ********************************************************************************************************************** + // Downlink Message + // ********************************************************************************************************************** + DOWNLINK_SEND_OF_HOVER(DefaultChannel, DefaultDevice, &FPS, &dx_sum, &dy_sum, &OFx, &OFy, &diff_roll, &diff_pitch, &Velx, &Vely, &V_body.x, &V_body.y, &cam_h, &count); +} + diff --git a/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.h b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.h new file mode 100644 index 00000000000..f46fcc0d77b --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.h @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2014 Hann Woei Ho + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_code.h + * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * + * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 + */ + +#ifndef _OPT_FL_LAND_H +#define _OPT_FL_LAND_H + +// Settable by pluging +extern unsigned int imgWidth, imgHeight; +extern unsigned int verbose; + +// Variables used by the controller +extern float Velx, Vely; +extern int count; +extern int flow_count; +extern struct FloatVect3 V_body; + +// Called by plugin +void my_plugin_init(void); +void my_plugin_run(unsigned char *frame); + +// Timer +void start_timer_rates(void); +long end_timer_rates(void); +#endif diff --git a/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.c b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.c new file mode 100644 index 00000000000..a254d05b1f0 --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.c @@ -0,0 +1,218 @@ +/* + * Copyright (C) 2014 Hann Woei Ho + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_module.h + * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * + * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 + */ + + +// Own header +#include "opticflow_module.h" + +// Navigate Based On Vision +#include "hover_stabilization.h" + +// Paparazzi +#include "state.h" // for attitude +#include "boards/ardrone/navdata.h" // for ultrasound Height + +// Threaded computer vision +#include + +// Frame Rate (FPS) +#include +float FPS; +volatile long timestamp; +#define USEC_PER_SEC 1000000L +long time_elapsed (struct timeval *t1, struct timeval *t2) +{ + long sec, usec; + sec = t2->tv_sec - t1->tv_sec; + usec = t2->tv_usec - t1->tv_usec; + if (usec < 0) + { + --sec; + usec = usec + USEC_PER_SEC; + } + return sec*USEC_PER_SEC + usec; +} + +struct timeval start_time; +struct timeval end_time; + +void start_timer() { + gettimeofday (&start_time, NULL); +} +long end_timer() { + gettimeofday (&end_time, NULL); + return time_elapsed(&start_time, &end_time); +} + +void opticflow_module_init(void) +{ + // Immediately start the vision thread when the module initialized + opticflow_module_start(); + + // Stabilization Code Initialization + init_hover_stabilization_onvision(); + + // Frame Rate Initialization + FPS = 0.0; + timestamp=0; + start_timer(); +} + + +volatile uint8_t computervision_thread_has_results = 0; + +void opticflow_module_run(void) +{ + + // Read Latest Vision Module Results + if (computervision_thread_has_results) + { + computervision_thread_has_results = 0; + run_hover_stabilization_onvision(); + } +} + +///////////////////////////////////////////////////////////////////////// +// COMPUTER VISION THREAD + +// Video +#include "v4l/video.h" +#include "resize.h" + +// Payload Code +#include "opticflow_code.h" + +// Downlink Video +//#define DOWNLINK_VIDEO 1 + +#ifdef DOWNLINK_VIDEO +#include "encoding/jpeg.h" +#include "encoding/rtp.h" +#endif + +#include + +pthread_t computervision_thread; +volatile uint8_t computervision_thread_status = 0; +volatile uint8_t computer_vision_thread_command = 0; +void *computervision_thread_main(void* data); +void *computervision_thread_main(void* data) +{ + // Video Input + struct vid_struct vid; + vid.device = (char*)"/dev/video2"; // video1 = front camera; video2 = bottom camera + vid.w=320; + vid.h=240; + vid.n_buffers = 4; + + if (video_init(&vid)<0) { + printf("Error initialising video\n"); + computervision_thread_status = -1; + return 0; + } + + // Video Grabbing + struct img_struct* img_new = video_create_image(&vid); + + // Video Resizing + #define DOWNSIZE_FACTOR 1 + struct img_struct small; + small.w = vid.w / DOWNSIZE_FACTOR; + small.h = vid.h / DOWNSIZE_FACTOR; + small.buf = (uint8_t*)malloc(small.w*small.h*2); + +#ifdef DOWNLINK_VIDEO + // Video Compression + uint8_t* jpegbuf = (uint8_t*)malloc(vid.h*vid.w*2); + + // Network Transmit + struct UdpSocket* vsock; + //#define FMS_UNICAST 0 + //#define FMS_BROADCAST 1 + vsock = udp_socket("192.168.1.255", 5000, 5001, FMS_BROADCAST); +#endif + + // First Apply Settings before init + imgWidth = small.w; + imgHeight = small.h; + verbose = 2; + my_plugin_init(); + + while (computer_vision_thread_command > 0) + { + video_grab_image(&vid, img_new); + + // FPS + timestamp = end_timer(); + FPS = (float) 1000000/(float)timestamp; + // printf("dt = %d, FPS = %f\n",timestamp, FPS); + start_timer(); + + // Resize + //resize_uyuv(img_new, &small, DOWNSIZE_FACTOR); + + // Run Image Processing + my_plugin_run(img_new->buf); + // my_plugin_run(small.buf); + + #ifdef DOWNLINK_VIDEO + // JPEG encode the image: + uint32_t quality_factor = 10; //20 if no resize, + uint8_t dri_header = 0; + uint32_t image_format = FOUR_TWO_TWO; // format (in jpeg.h) + uint8_t* end = encode_image (small.buf, jpegbuf, quality_factor, image_format, small.w, small.h, dri_header); + uint32_t size = end-(jpegbuf); + + printf("Sending an image ...%u\n",size); + uint32_t delta_t_per_frame = 0; // 0 = use drone clock + send_rtp_frame(vsock, jpegbuf,size, small.w, small.h,0, quality_factor, dri_header, delta_t_per_frame); + #endif + computervision_thread_has_results++; + } + printf("Thread Closed\n"); + video_close(&vid); + computervision_thread_status = -100; + return 0; +} + +void opticflow_module_start(void) +{ + computer_vision_thread_command = 1; + int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main, NULL); + if(rc) { + printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc); + } +} + +void opticflow_module_stop(void) +{ + computer_vision_thread_command = 0; +} + + + diff --git a/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.h b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.h new file mode 100644 index 00000000000..b5e635a1ff1 --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.h @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2014 Hann Woei Ho + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_module.h + * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * + * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 + */ + +#ifndef OPTICFLOW_LAND_H +#define OPTICFLOW_LAND_H + +// Module functions +extern void opticflow_module_init(void); +extern void opticflow_module_run(void); +extern void opticflow_module_start(void); +extern void opticflow_module_stop(void); + +// Frame Rate +extern float FPS; +struct timeval; +long time_elapsed(struct timeval *t1, struct timeval *t2); +void start_timer(void); +long end_timer(void); + +#endif /* OPTICFLOW_LAND_H */ diff --git a/sw/airborne/modules/computer_vision/OpticFlow/video_message_structs.h b/sw/airborne/modules/computer_vision/OpticFlow/video_message_structs.h new file mode 100644 index 00000000000..2039dd4edd6 --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/video_message_structs.h @@ -0,0 +1,32 @@ +#ifndef VMSTRTS_H +#define VMSTRTS_H + +/*** + an exact copy of this file to exist in ppz + this files keeps the structs that are serialized and streamed over tcp/ip through localhost + */ + +#define N_BINS 10 + +struct gst2ppz_message_struct +{ + unsigned int ID; // Keep different modules for using each others data + unsigned int counter; // counter to keep track of data + unsigned int obstacle_bins[N_BINS]; // optical flow output, shift in x direction + unsigned int uncertainty_bins[N_BINS]; //optical flow output, shift in y direction +}; +extern struct gst2ppz_message_struct gst2ppz; + +struct ppz2gst_message_struct +{ + unsigned int ID; // Keep different modules for using each others data + unsigned int counter; //counter to keep track of data + int pitch; + int roll; + int alt; + int adjust_factor; // 0-10 :adjust brightness +}; +extern struct ppz2gst_message_struct ppz2gst; + +#endif /* VMSTRTS_H */ + diff --git a/sw/airborne/modules/computer_vision/cv/color.h b/sw/airborne/modules/computer_vision/cv/color.h index 46ffdb413a8..86737831f14 100644 --- a/sw/airborne/modules/computer_vision/cv/color.h +++ b/sw/airborne/modules/computer_vision/cv/color.h @@ -40,10 +40,8 @@ inline void grayscale_uyvy(struct img_struct *input, struct img_struct *output) } } -inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, - uint8_t u_M, uint8_t v_m, uint8_t v_M); -inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, - uint8_t u_M, uint8_t v_m, uint8_t v_M) +inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M); +inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M) { int cnt = 0; uint8_t *source = input->buf; diff --git a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c index 119f4bc2cf2..7d336dd1d6f 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c +++ b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c @@ -433,8 +433,7 @@ void MakeTables(int q) -uint8_t *encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_factor, uint32_t image_format, - uint32_t image_width, uint32_t image_height, uint8_t add_dri_header) +uint8_t *encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_factor, uint32_t image_format, uint32_t image_width, uint32_t image_height, uint8_t add_dri_header) { uint16_t i, j; diff --git a/sw/airborne/modules/computer_vision/cv/encoding/rtp.c b/sw/airborne/modules/computer_vision/cv/encoding/rtp.c index 22211268186..541da0068e0 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/rtp.c +++ b/sw/airborne/modules/computer_vision/cv/encoding/rtp.c @@ -7,9 +7,7 @@ #include "rtp.h" -void send_rtp_packet(struct UdpSocket *sock, uint8_t *Jpeg, int JpegLen, uint32_t m_SequenceNumber, - uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code, - uint8_t has_dri_header); +void send_rtp_packet(struct UdpSocket *sock, uint8_t *Jpeg, int JpegLen, uint32_t m_SequenceNumber, uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header); // http://www.ietf.org/rfc/rfc3550.txt @@ -51,19 +49,16 @@ void test_rtp_frame(struct UdpSocket *sock) uint8_t quality_code = 0x54; if (toggle) { - send_rtp_packet(sock, JpegScanDataCh2A, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, - quality_code, 0); + send_rtp_packet(sock, JpegScanDataCh2A, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0); } else { - send_rtp_packet(sock, JpegScanDataCh2B, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, - quality_code, 0); + send_rtp_packet(sock, JpegScanDataCh2B, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0); } framecounter++; timecounter += 3600; } -void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int w, int h, uint8_t format_code, - uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t) +void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t) { static uint32_t packetcounter = 0; static uint32_t timecounter = 0; @@ -88,8 +83,7 @@ void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int len = JpegLen; } - send_rtp_packet(sock, Jpeg, len, packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code, - has_dri_header); + send_rtp_packet(sock, Jpeg, len, packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code, has_dri_header); JpegLen -= len; Jpeg += len; diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/fast9/LICENSE b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/LICENSE new file mode 100644 index 00000000000..ee48fe68090 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/LICENSE @@ -0,0 +1,30 @@ +Copyright (c) 2006, 2008 Edward Rosten +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + + + *Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + *Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + *Neither the name of the University of Cambridge nor the names of + its contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.c b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.c new file mode 100644 index 00000000000..9601a02e8e0 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.c @@ -0,0 +1,6069 @@ +/* +Copyright (c) 2006, 2008 Edward Rosten +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + + + *Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + *Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + *Neither the name of the University of Cambridge nor the names of + its contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include "fastRosten.h" + +#define Compare(X, Y) ((X)>=(Y)) + +xyFAST* fast9_detect_nonmax(const byte* im, int xsize, int ysize, int stride, int b, int* ret_num_corners) +{ + xyFAST* corners; + int num_corners; + int* scores; + xyFAST* nonmax; + + corners = fast9_detect(im, xsize, ysize, stride, b, &num_corners); + scores = fast9_score(im, stride, corners, num_corners, b); + nonmax = nonmax_suppression(corners, scores, num_corners, ret_num_corners); + + free(corners); + free(scores); + + return nonmax; +} + +xyFAST* nonmax_suppression(const xyFAST* corners, const int* scores, int num_corners, int* ret_num_nonmax) +{ + int num_nonmax=0; + int last_row; + int* row_start; + int i, j; + xyFAST* ret_nonmax; + const int sz = (int)num_corners; + + /*Point above points (roughly) to the pixel above the one of interest, if there + is a feature there.*/ + int point_above = 0; + int point_below = 0; + + + if(num_corners < 1) + { + *ret_num_nonmax = 0; + return 0; + } + + ret_nonmax = (xyFAST*)malloc(num_corners * sizeof(xyFAST)); + + /* Find where each row begins + (the corners are output in raster scan order). A beginning of -1 signifies + that there are no corners on that row. */ + last_row = corners[num_corners-1].y; + row_start = (int*)malloc((last_row+1)*sizeof(int)); + + for(i=0; i < last_row+1; i++) + row_start[i] = -1; + + { + int prev_row = -1; + for(i=0; i< num_corners; i++) + if(corners[i].y != prev_row) + { + row_start[corners[i].y] = i; + prev_row = corners[i].y; + } + } + + + + for(i=0; i < sz; i++) + { + int score = scores[i]; + xyFAST pos = corners[i]; + + /*Check left */ + if(i > 0) + if(corners[i-1].x == pos.x-1 && corners[i-1].y == pos.y && Compare(scores[i-1], score)) + continue; + + /*Check right*/ + if(i < (sz - 1)) + if(corners[i+1].x == pos.x+1 && corners[i+1].y == pos.y && Compare(scores[i+1], score)) + continue; + + /*Check above (if there is a valid row above)*/ + if(pos.y != 0 && row_start[pos.y - 1] != -1) + { + /*Make sure that current point_above is one + row above.*/ + if(corners[point_above].y < pos.y - 1) + point_above = row_start[pos.y-1]; + + /*Make point_above point to the first of the pixels above the current point, + if it exists.*/ + for(; corners[point_above].y < pos.y && corners[point_above].x < pos.x - 1; point_above++) + {} + + + for(j=point_above; corners[j].y < pos.y && corners[j].x <= pos.x + 1; j++) + { + int x = corners[j].x; + if( (x == pos.x - 1 || x ==pos.x || x == pos.x+1) && Compare(scores[j], score)) + goto cont; + } + + } + + /*Check below (if there is anything below)*/ + if(pos.y != last_row && row_start[pos.y + 1] != -1 && point_below < sz) /*Nothing below*/ + { + if(corners[point_below].y < pos.y + 1) + point_below = row_start[pos.y+1]; + + /* Make point below point to one of the pixels belowthe current point, if it + exists.*/ + for(; point_below < sz && corners[point_below].y == pos.y+1 && corners[point_below].x < pos.x - 1; point_below++) + {} + + for(j=point_below; j < sz && corners[j].y == pos.y+1 && corners[j].x <= pos.x + 1; j++) + { + int x = corners[j].x; + if( (x == pos.x - 1 || x ==pos.x || x == pos.x+1) && Compare(scores[j],score)) + goto cont; + } + } + + ret_nonmax[num_nonmax++] = corners[i]; + cont: + ; + } + + free(row_start); + *ret_num_nonmax = num_nonmax; + return ret_nonmax; +} + +int fast9_corner_score(const byte* p, const int pixel[], int bstart) +{ + int bmin = bstart; + int bmax = 255; + int b = (bmax + bmin)/2; + + /*Compute the score using binary search*/ + for(;;) + { + int cb = *p + b; + int c_b= *p - b; + + + if( p[pixel[0]] > cb) + if( p[pixel[1]] > cb) + if( p[pixel[2]] > cb) + if( p[pixel[3]] > cb) + if( p[pixel[4]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + goto is_a_corner; + else + if( p[pixel[15]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[7]] < c_b) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[14]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[6]] < c_b) + if( p[pixel[15]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[13]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[14]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[13]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[5]] < c_b) + if( p[pixel[14]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[12]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[13]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[14]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[6]] < c_b) + goto is_a_corner; + else + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[12]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[6]] < c_b) + goto is_a_corner; + else + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[4]] < c_b) + if( p[pixel[13]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[11]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[12]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[13]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[5]] < c_b) + goto is_a_corner; + else + if( p[pixel[14]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[11]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[5]] < c_b) + goto is_a_corner; + else + if( p[pixel[14]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[3]] < c_b) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[4]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[10]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[4]] < c_b) + goto is_a_corner; + else + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[4]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[10]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[4]] < c_b) + goto is_a_corner; + else + if( p[pixel[13]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[2]] < c_b) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[4]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[3]] > cb) + if( p[pixel[4]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[9]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[4]] < c_b) + if( p[pixel[3]] < c_b) + goto is_a_corner; + else + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[4]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[3]] > cb) + if( p[pixel[4]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[9]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[4]] < c_b) + if( p[pixel[3]] < c_b) + goto is_a_corner; + else + if( p[pixel[12]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[1]] < c_b) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[4]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[3]] > cb) + if( p[pixel[4]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[2]] > cb) + if( p[pixel[3]] > cb) + if( p[pixel[4]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[8]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[4]] < c_b) + if( p[pixel[3]] < c_b) + if( p[pixel[2]] < c_b) + goto is_a_corner; + else + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[4]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[3]] > cb) + if( p[pixel[4]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[2]] > cb) + if( p[pixel[3]] > cb) + if( p[pixel[4]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[8]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[4]] < c_b) + if( p[pixel[3]] < c_b) + if( p[pixel[2]] < c_b) + goto is_a_corner; + else + if( p[pixel[11]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[0]] < c_b) + if( p[pixel[1]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[4]] > cb) + if( p[pixel[3]] > cb) + if( p[pixel[2]] > cb) + goto is_a_corner; + else + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[4]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[3]] < c_b) + if( p[pixel[4]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[2]] < c_b) + if( p[pixel[3]] < c_b) + if( p[pixel[4]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[1]] < c_b) + if( p[pixel[2]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[4]] > cb) + if( p[pixel[3]] > cb) + goto is_a_corner; + else + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[4]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[3]] < c_b) + if( p[pixel[4]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[2]] < c_b) + if( p[pixel[3]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[4]] > cb) + goto is_a_corner; + else + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[4]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[3]] < c_b) + if( p[pixel[4]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[5]] > cb) + goto is_a_corner; + else + if( p[pixel[14]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[13]] < c_b) + if( p[pixel[11]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[12]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[4]] < c_b) + if( p[pixel[5]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[6]] > cb) + goto is_a_corner; + else + if( p[pixel[15]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[14]] < c_b) + if( p[pixel[12]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[13]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[6]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[5]] < c_b) + if( p[pixel[6]] > cb) + if( p[pixel[15]] < c_b) + if( p[pixel[13]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[14]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[6]] < c_b) + if( p[pixel[7]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + goto is_a_corner; + else + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[13]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[12]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[6]] > cb) + goto is_a_corner; + else + if( p[pixel[15]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[11]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[5]] > cb) + goto is_a_corner; + else + if( p[pixel[14]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[4]] > cb) + goto is_a_corner; + else + if( p[pixel[13]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[4]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[9]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[4]] > cb) + if( p[pixel[3]] > cb) + goto is_a_corner; + else + if( p[pixel[12]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[4]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[3]] < c_b) + if( p[pixel[4]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[8]] > cb) + if( p[pixel[7]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[10]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[4]] > cb) + if( p[pixel[3]] > cb) + if( p[pixel[2]] > cb) + goto is_a_corner; + else + if( p[pixel[11]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[4]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[3]] < c_b) + if( p[pixel[4]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[2]] < c_b) + if( p[pixel[3]] < c_b) + if( p[pixel[4]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[7]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[7]] > cb) + if( p[pixel[8]] > cb) + if( p[pixel[9]] > cb) + if( p[pixel[6]] > cb) + if( p[pixel[5]] > cb) + if( p[pixel[4]] > cb) + if( p[pixel[3]] > cb) + if( p[pixel[2]] > cb) + if( p[pixel[1]] > cb) + goto is_a_corner; + else + if( p[pixel[10]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] > cb) + if( p[pixel[11]] > cb) + if( p[pixel[12]] > cb) + if( p[pixel[13]] > cb) + if( p[pixel[14]] > cb) + if( p[pixel[15]] > cb) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else if( p[pixel[7]] < c_b) + if( p[pixel[8]] < c_b) + if( p[pixel[9]] < c_b) + if( p[pixel[6]] < c_b) + if( p[pixel[5]] < c_b) + if( p[pixel[4]] < c_b) + if( p[pixel[3]] < c_b) + if( p[pixel[2]] < c_b) + if( p[pixel[1]] < c_b) + goto is_a_corner; + else + if( p[pixel[10]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + if( p[pixel[10]] < c_b) + if( p[pixel[11]] < c_b) + if( p[pixel[12]] < c_b) + if( p[pixel[13]] < c_b) + if( p[pixel[14]] < c_b) + if( p[pixel[15]] < c_b) + goto is_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + else + goto is_not_a_corner; + + is_a_corner: + bmin=b; + goto end_if; + + is_not_a_corner: + bmax=b; + goto end_if; + + end_if: + + if(bmin == bmax - 1 || bmin == bmax) + return bmin; + b = (bmin + bmax) / 2; + } +} + +static void make_offsets(int pixel[], int row_stride) +{ + pixel[0] = 0 + row_stride * 3; + pixel[1] = 1 + row_stride * 3; + pixel[2] = 2 + row_stride * 2; + pixel[3] = 3 + row_stride * 1; + pixel[4] = 3 + row_stride * 0; + pixel[5] = 3 + row_stride * -1; + pixel[6] = 2 + row_stride * -2; + pixel[7] = 1 + row_stride * -3; + pixel[8] = 0 + row_stride * -3; + pixel[9] = -1 + row_stride * -3; + pixel[10] = -2 + row_stride * -2; + pixel[11] = -3 + row_stride * -1; + pixel[12] = -3 + row_stride * 0; + pixel[13] = -3 + row_stride * 1; + pixel[14] = -2 + row_stride * 2; + pixel[15] = -1 + row_stride * 3; +} + + + +int* fast9_score(const byte* i, int stride, xyFAST* corners, int num_corners, int b) +{ + int* scores = (int*)malloc(sizeof(int)* num_corners); + int n; + + int pixel[16]; + make_offsets(pixel, stride); + + for(n=0; n < num_corners; n++) + scores[n] = fast9_corner_score(i + corners[n].y*stride + corners[n].x, pixel, b); + + return scores; +} + + +xyFAST* fast9_detect(const byte* im, int xsize, int ysize, int stride, int b, int* ret_num_corners) +{ + int num_corners=0; + xyFAST* ret_corners; + int rsize=512; + int pixel[16]; + int x, y; + + ret_corners = (xyFAST*)malloc(sizeof(xyFAST)*rsize); + make_offsets(pixel, stride); + + for(y=3; y < ysize - 3; y++) + for(x=3; x < xsize - 3; x++) + { + const byte* p = im + y*stride + x; + + int cb = *p + b; + int c_b= *p - b; + if(p[pixel[0]] > cb) + if(p[pixel[1]] > cb) + if(p[pixel[2]] > cb) + if(p[pixel[3]] > cb) + if(p[pixel[4]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + {} + else + if(p[pixel[15]] > cb) + {} + else + continue; + else if(p[pixel[7]] < c_b) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + continue; + else if(p[pixel[14]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + continue; + else + continue; + else if(p[pixel[6]] < c_b) + if(p[pixel[15]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + {} + else + continue; + else if(p[pixel[13]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[14]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + continue; + else + continue; + else if(p[pixel[13]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[5]] < c_b) + if(p[pixel[14]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[15]] > cb) + {} + else + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[12]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[13]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[14]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[6]] < c_b) + {} + else + if(p[pixel[15]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[12]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[6]] < c_b) + {} + else + if(p[pixel[15]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[4]] < c_b) + if(p[pixel[13]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[11]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[12]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[13]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[5]] < c_b) + {} + else + if(p[pixel[14]] < c_b) + {} + else + continue; + else + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[11]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[5]] < c_b) + {} + else + if(p[pixel[14]] < c_b) + {} + else + continue; + else + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[3]] < c_b) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[4]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[10]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[4]] < c_b) + {} + else + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + {} + else + continue; + else + continue; + else + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[4]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[10]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[4]] < c_b) + {} + else + if(p[pixel[13]] < c_b) + {} + else + continue; + else + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + {} + else + continue; + else + continue; + else + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[2]] < c_b) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[4]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[3]] > cb) + if(p[pixel[4]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[9]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[4]] < c_b) + if(p[pixel[3]] < c_b) + {} + else + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + {} + else + continue; + else + continue; + else + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[4]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[3]] > cb) + if(p[pixel[4]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[9]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[4]] < c_b) + if(p[pixel[3]] < c_b) + {} + else + if(p[pixel[12]] < c_b) + {} + else + continue; + else + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + {} + else + continue; + else + continue; + else + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[1]] < c_b) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + {} + else + continue; + else + continue; + else + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[4]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[3]] > cb) + if(p[pixel[4]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[2]] > cb) + if(p[pixel[3]] > cb) + if(p[pixel[4]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[8]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[4]] < c_b) + if(p[pixel[3]] < c_b) + if(p[pixel[2]] < c_b) + {} + else + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + {} + else + continue; + else + continue; + else + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + {} + else + continue; + else + continue; + else + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[4]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[3]] > cb) + if(p[pixel[4]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[2]] > cb) + if(p[pixel[3]] > cb) + if(p[pixel[4]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[8]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[4]] < c_b) + if(p[pixel[3]] < c_b) + if(p[pixel[2]] < c_b) + {} + else + if(p[pixel[11]] < c_b) + {} + else + continue; + else + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + {} + else + continue; + else + continue; + else + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[0]] < c_b) + if(p[pixel[1]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[4]] > cb) + if(p[pixel[3]] > cb) + if(p[pixel[2]] > cb) + {} + else + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + {} + else + continue; + else + continue; + else + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + {} + else + continue; + else + continue; + else + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[4]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[3]] < c_b) + if(p[pixel[4]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[2]] < c_b) + if(p[pixel[3]] < c_b) + if(p[pixel[4]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[1]] < c_b) + if(p[pixel[2]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[4]] > cb) + if(p[pixel[3]] > cb) + {} + else + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + {} + else + continue; + else + continue; + else + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[4]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[3]] < c_b) + if(p[pixel[4]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[2]] < c_b) + if(p[pixel[3]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[4]] > cb) + {} + else + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + {} + else + continue; + else + continue; + else + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[4]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[3]] < c_b) + if(p[pixel[4]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[5]] > cb) + {} + else + if(p[pixel[14]] > cb) + {} + else + continue; + else + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[13]] < c_b) + if(p[pixel[11]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[12]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[5]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[4]] < c_b) + if(p[pixel[5]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[6]] > cb) + {} + else + if(p[pixel[15]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[14]] < c_b) + if(p[pixel[12]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[13]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[6]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[5]] < c_b) + if(p[pixel[6]] > cb) + if(p[pixel[15]] < c_b) + if(p[pixel[13]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[14]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + {} + else + continue; + else + continue; + else + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[6]] < c_b) + if(p[pixel[7]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[15]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + continue; + else + continue; + else if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + {} + else + if(p[pixel[15]] < c_b) + {} + else + continue; + else + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + continue; + else + continue; + else + if(p[pixel[13]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[12]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[6]] > cb) + {} + else + if(p[pixel[15]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[11]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[5]] > cb) + {} + else + if(p[pixel[14]] > cb) + {} + else + continue; + else + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[10]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[4]] > cb) + {} + else + if(p[pixel[13]] > cb) + {} + else + continue; + else + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + {} + else + continue; + else + continue; + else + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[4]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[9]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[4]] > cb) + if(p[pixel[3]] > cb) + {} + else + if(p[pixel[12]] > cb) + {} + else + continue; + else + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + {} + else + continue; + else + continue; + else + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[4]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[3]] < c_b) + if(p[pixel[4]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[8]] > cb) + if(p[pixel[7]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[10]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[4]] > cb) + if(p[pixel[3]] > cb) + if(p[pixel[2]] > cb) + {} + else + if(p[pixel[11]] > cb) + {} + else + continue; + else + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + {} + else + continue; + else + continue; + else + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + {} + else + continue; + else + continue; + else + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[4]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[3]] < c_b) + if(p[pixel[4]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[2]] < c_b) + if(p[pixel[3]] < c_b) + if(p[pixel[4]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[7]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[7]] > cb) + if(p[pixel[8]] > cb) + if(p[pixel[9]] > cb) + if(p[pixel[6]] > cb) + if(p[pixel[5]] > cb) + if(p[pixel[4]] > cb) + if(p[pixel[3]] > cb) + if(p[pixel[2]] > cb) + if(p[pixel[1]] > cb) + {} + else + if(p[pixel[10]] > cb) + {} + else + continue; + else + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + {} + else + continue; + else + continue; + else + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[10]] > cb) + if(p[pixel[11]] > cb) + if(p[pixel[12]] > cb) + if(p[pixel[13]] > cb) + if(p[pixel[14]] > cb) + if(p[pixel[15]] > cb) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else if(p[pixel[7]] < c_b) + if(p[pixel[8]] < c_b) + if(p[pixel[9]] < c_b) + if(p[pixel[6]] < c_b) + if(p[pixel[5]] < c_b) + if(p[pixel[4]] < c_b) + if(p[pixel[3]] < c_b) + if(p[pixel[2]] < c_b) + if(p[pixel[1]] < c_b) + {} + else + if(p[pixel[10]] < c_b) + {} + else + continue; + else + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + {} + else + continue; + else + continue; + else + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + if(p[pixel[10]] < c_b) + if(p[pixel[11]] < c_b) + if(p[pixel[12]] < c_b) + if(p[pixel[13]] < c_b) + if(p[pixel[14]] < c_b) + if(p[pixel[15]] < c_b) + {} + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + else + continue; + if(num_corners == rsize) + { + rsize*=2; + ret_corners = (xyFAST*)realloc(ret_corners, sizeof(xyFAST)*rsize); + } + ret_corners[num_corners].x = x; + ret_corners[num_corners].y = y; + num_corners++; + + } + + *ret_num_corners = num_corners; + return ret_corners; + +} + diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.h b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.h new file mode 100644 index 00000000000..88d25714909 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.h @@ -0,0 +1,51 @@ +/* +Copyright (c) 2006, 2008 Edward Rosten +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + + + *Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + *Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + *Neither the name of the University of Cambridge nor the names of + its contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef FAST_H +#define FAST_H + +typedef struct { int x, y; } xyFAST; +typedef unsigned char byte; + +int fast9_corner_score(const byte* p, const int pixel[], int bstart); + +xyFAST* fast9_detect(const byte* im, int xsize, int ysize, int stride, int b, int* ret_num_corners); + +int* fast9_score(const byte* i, int stride, xyFAST* corners, int num_corners, int b); + +xyFAST* fast9_detect_nonmax(const byte* im, int xsize, int ysize, int stride, int b, int* ret_num_corners); + +xyFAST* nonmax_suppression(const xyFAST* corners, const int* scores, int num_corners, int* ret_num_nonmax); + + +#endif diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c new file mode 100644 index 00000000000..f46243b9648 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c @@ -0,0 +1,624 @@ +/* + * Copyright (C) 2014 Hann Woei Ho + * + * - Initial fixed-point C implementation by G. de Croon + * - Algorithm: Lucas-Kanade by Yves Bouguet + * - Publication: http://robots.stanford.edu/cs223b04/algo_tracking.pdf + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * @file paparazzi/sw/ext/ardrone2_vision/cv/opticflow/optic_flow_ardrone.c + * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * + * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 + */ + +#include +#include +#include +#include +#include "optic_flow_gdc.h" +#include "../../modules/OpticFlow/opticflow_module.h" + +#define int_index(x,y) (y * IMG_WIDTH + x) +#define uint_index(xx, yy) (((yy * IMG_WIDTH + xx) * 2) & 0xFFFFFFFC) +#define NO_MEMORY -1 +#define OK 0 +#define N_VISUAL_INPUTS 51 +#define N_ACTIONS 3 +#define MAX_COUNT_PT 50 + +unsigned int IMG_WIDTH, IMG_HEIGHT; + +void multiplyImages(int *ImA, int *ImB, int *ImC, int width, int height) +{ + int x, y; + unsigned int ix; + + // printf("W = %d, H = %d\n\r", IMG_WIDTH, IMG_HEIGHT); + + for (x = 0; x < width; x++) { + for (y = 0; y < height; y++) { + ix = (y * width + x); + ImC[ix] = ImA[ix] * ImB[ix]; + // If we want to keep the values in [0, 255]: + // ImC[ix] /= 255; + } + } +} + +void getImageDifference(int *ImA, int *ImB, int *ImC, int width, int height) +{ + int x, y; + unsigned int ix; + + // printf("W = %d, H = %d\n\r", IMG_WIDTH, IMG_HEIGHT); + + for (x = 0; x < width; x++) { + for (y = 0; y < height; y++) { + ix = (y * width + x); + ImC[ix] = ImA[ix] - ImB[ix]; + } + } + +} + +void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int center_y, int half_window_size, int subpixel_factor) +{ + int x, y, x_0, y_0, x_0_or, y_0_or, i, j, window_size, alpha_x, alpha_y, max_x, max_y; + //int printed, limit; + unsigned int ix1, ix2, Y; + window_size = half_window_size * 2 + 1; + max_x = (IMG_WIDTH - 1) * subpixel_factor; + max_y = (IMG_HEIGHT - 1) * subpixel_factor; + //printed = 0; limit = 4; + + for (i = 0; i < window_size; i++) { + for (j = 0; j < window_size; j++) { + // index for this position in the patch: + ix1 = (j * window_size + i); + + // determine subpixel coordinates of the current pixel: + x = center_x + (i - half_window_size) * subpixel_factor; + if (x < 0) { x = 0; } + if (x > max_x) { x = max_x; } + y = center_y + (j - half_window_size) * subpixel_factor; + if (y < 0) { y = 0; } + if (y > max_y) { y = max_y; } + // pixel to the top left: + x_0_or = (x / subpixel_factor); + x_0 = x_0_or * subpixel_factor; + y_0_or = (y / subpixel_factor); + y_0 = y_0_or * subpixel_factor; + /*if(printed < limit) + { + printf("x_0_or = %d, y_0_or = %d;\n\r", x_0_or, y_0_or); + printf("x_0 = %d, y_0 = %d\n\r"); + printed++; + }*/ + + + if (x == x_0 && y == y_0) { + // simply copy the pixel: +// ix2 = uint_index(x_0_or, y_0_or); +// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1; + ix2 = y_0_or * IMG_WIDTH + x_0_or; + Y = (unsigned int)frame_buf[ix2 + 1]; + Patch[ix1] = (int) Y; + } else { + // blending according to how far the subpixel coordinates are from the pixel coordinates + alpha_x = (x - x_0); + alpha_y = (y - y_0); + + // the patch pixel is a blend from the four surrounding pixels: + ix2 = y_0_or * IMG_WIDTH + x_0_or; + Y = (unsigned int)frame_buf[ix2 + 1]; +// ix2 = uint_index(x_0_or, y_0_or); +// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1; + Patch[ix1] = (subpixel_factor - alpha_x) * (subpixel_factor - alpha_y) * ((int) Y); + + ix2 = y_0_or * IMG_WIDTH + (x_0_or + 1); + Y = (unsigned int)frame_buf[ix2 + 1]; +// ix2 = uint_index((x_0_or+1), y_0_or); +// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1; + //if(printed < limit) printf("subpixel: TR = %d\n\r", Y); + Patch[ix1] += alpha_x * (subpixel_factor - alpha_y) * ((int) Y); + + ix2 = (y_0_or + 1) * IMG_WIDTH + x_0_or; + Y = (unsigned int)frame_buf[ix2 + 1]; +// ix2 = uint_index(x_0_or, (y_0_or+1)); +// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1; + //if(printed < limit) printf("subpixel: BL = %d\n\r", Y); + Patch[ix1] += (subpixel_factor - alpha_x) * alpha_y * ((int) Y); + + ix2 = (y_0_or + 1) * IMG_WIDTH + (x_0_or + 1); + Y = (unsigned int)frame_buf[ix2 + 1]; +// ix2 = uint_index((x_0_or+1), (y_0_or+1)); +// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1; + //if(printed < limit) printf("subpixel: BR = %d\n\r", Y); + Patch[ix1] += alpha_x * alpha_y * ((int) Y); + + // normalize patch value + Patch[ix1] /= (subpixel_factor * subpixel_factor); + + /*if(printed < limit) + { + + printf("alpha_x = %d, alpha_y = %d, x_0 = %d, y_0 = %d, x = %d, y = %d, Patch[ix1] = %d\n\r", alpha_x, alpha_y, x_0, y_0, x, y, Patch[ix1]); + // printed++; + } + */ + + } + } + } + + return; +} + +void getGradientPatch(int *Patch, int *DX, int *DY, int half_window_size) +{ + unsigned int ix1, ix2; + int x, y, padded_patch_size, patch_size, Y1, Y2; + // int printed; printed = 0; + + padded_patch_size = 2 * (half_window_size + 1) + 1; + patch_size = 2 * half_window_size + 1; + // currently we use [0 0 0; -1 0 1; 0 0 0] as mask for dx + for (x = 1; x < padded_patch_size - 1; x++) { + for (y = 1; y < padded_patch_size - 1; y++) { + // index in DX, DY: + ix2 = (unsigned int)((y - 1) * patch_size + (x - 1)); + + ix1 = (unsigned int)(y * padded_patch_size + x - 1); + Y1 = Patch[ix1]; + ix1 = (unsigned int)(y * padded_patch_size + x + 1); + Y2 = Patch[ix1]; +// DX[ix2] = Y2 - Y1; + DX[ix2] = (Y2 - Y1) / 2; + + ix1 = (unsigned int)((y - 1) * padded_patch_size + x); + Y1 = Patch[ix1]; + ix1 = (unsigned int)((y + 1) * padded_patch_size + x); + Y2 = Patch[ix1]; +// DY[ix2] = Y2 - Y1; + DY[ix2] = (Y2 - Y1) / 2; + + /*if(printed < 1 && DX[ix2] > 0) + { + printf("DX = %d, DY = %d\n\r", DX[ix2], DY[ix2]); + printed++; + } + else if(printed == 1 && DX[ix2] < 0) + { + printf("DX = %d, DY = %d\n\r", DX[ix2], DY[ix2]); + printed++; + }*/ + + + } + } + + return; +} + +int getSumPatch(int *Patch, int size) +{ + int x, y, sum; // , threshold + unsigned int ix; + + // in order to keep the sum within range: + //threshold = 50000; // typical values are far below this threshold + + sum = 0; + for (x = 0; x < size; x++) { + for (y = 0; y < size; y++) { + ix = (y * size) + x; + //if(sum < threshold && sum > -threshold) + //{ + sum += Patch[ix]; // do not check thresholds + //} + /*else + { + if(sum > threshold) + { + sum = threshold; + } + else + { + sum = -threshold; + } + }*/ + } + } + + return sum; +} + +int calculateG(int *G, int *DX, int *DY, int half_window_size) +{ + int patch_size; + int *DXX; int *DXY; int *DYY; + + patch_size = 2 * half_window_size + 1; + + // allocate memory: + DXX = (int *) malloc(patch_size * patch_size * sizeof(int)); + DXY = (int *) malloc(patch_size * patch_size * sizeof(int)); + DYY = (int *) malloc(patch_size * patch_size * sizeof(int)); + + if (DXX == 0 || DXY == 0 || DYY == 0) { + return NO_MEMORY; + } + + // then determine the second order gradients + multiplyImages(DX, DX, DXX, patch_size, patch_size); + multiplyImages(DX, DY, DXY, patch_size, patch_size); + multiplyImages(DY, DY, DYY, patch_size, patch_size); + + // calculate G: + G[0] = getSumPatch(DXX, patch_size); + G[1] = getSumPatch(DXY, patch_size); + G[2] = G[1]; + G[3] = getSumPatch(DYY, patch_size); + + // free memory: + free((char *) DXX); free((char *) DXY); free((char *) DYY); + + // no errors: + return OK; +} + + + +int calculateError(int *ImC, int width, int height) +{ + int x, y, error; + unsigned int ix; + + error = 0; + + for (x = 0; x < width; x++) { + for (y = 0; y < height; y++) { + ix = (y * width + x); + error += ImC[ix] * ImC[ix]; + } + } + + return error; +} + +int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int *p_x, int *p_y, int n_found_points, int imW, int imH, int *new_x, int *new_y, int *status, int half_window_size, int max_iterations) +{ + // A straightforward one-level implementation of Lucas-Kanade. + // For all points: + // (1) determine the subpixel neighborhood in the old image + // (2) get the x- and y- gradients + // (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window + // (4) iterate over taking steps in the image to minimize the error: + // [a] get the subpixel neighborhood in the new image + // [b] determine the image difference between the two neighborhoods + // [c] calculate the 'b'-vector + // [d] calculate the additional flow step and possibly terminate the iteration + int p, subpixel_factor, x, y, it, step_threshold, step_x, step_y, v_x, v_y, Det; + int b_x, b_y, patch_size, padded_patch_size, error; + unsigned int ix1, ix2; + int *I_padded_neighborhood; int *I_neighborhood; int *J_neighborhood; + int *DX; int *DY; int *ImDiff; int *IDDX; int *IDDY; + int G[4]; + int error_threshold; + + // set the image width and height + IMG_WIDTH = imW; + IMG_HEIGHT = imH; + // spatial resolution of flow is 1 / subpixel_factor + subpixel_factor = 10; + // determine patch sizes and initialize neighborhoods + patch_size = (2 * half_window_size + 1); + error_threshold = (25 * 25) * (patch_size * patch_size); + + padded_patch_size = (2 * half_window_size + 3); + I_padded_neighborhood = (int *) malloc(padded_patch_size * padded_patch_size * sizeof(int)); + I_neighborhood = (int *) malloc(patch_size * patch_size * sizeof(int)); + J_neighborhood = (int *) malloc(patch_size * patch_size * sizeof(int)); + if (I_padded_neighborhood == 0 || I_neighborhood == 0 || J_neighborhood == 0) { + return NO_MEMORY; + } + + DX = (int *) malloc(patch_size * patch_size * sizeof(int)); + DY = (int *) malloc(patch_size * patch_size * sizeof(int)); + IDDX = (int *) malloc(patch_size * patch_size * sizeof(int)); + IDDY = (int *) malloc(patch_size * patch_size * sizeof(int)); + ImDiff = (int *) malloc(patch_size * patch_size * sizeof(int)); + if (DX == 0 || DY == 0 || ImDiff == 0 || IDDX == 0 || IDDY == 0) { + return NO_MEMORY; + } + + for (p = 0; p < n_found_points; p++) { + //printf("*** NEW POINT ***\n\r"); + // status: point is not yet lost: + status[p] = 1; + + //printf("Normal coordinate: (%d,%d)\n\r", p_x[p], p_y[p]); + // We want to be able to take steps in the image of 1 / subpixel_factor: + p_x[p] *= subpixel_factor; + p_y[p] *= subpixel_factor; + //printf("Subpixel coordinate: (%d,%d)\n\r", p_x[p], p_y[p]); + + // if the pixel is outside the ROI in the image, do not track it: + if (!(p_x[p] > ((half_window_size + 1) * subpixel_factor) && p_x[p] < (IMG_WIDTH - half_window_size) * subpixel_factor && p_y[p] > ((half_window_size + 1) * subpixel_factor) && p_y[p] < (IMG_HEIGHT - half_window_size)*subpixel_factor)) { +// printf("Outside of ROI, P1[%d,%d]\n\r",p_x[p],p_y[p]); + status[p] = 0; + } + + // (1) determine the subpixel neighborhood in the old image + // we determine a padded neighborhood with the aim of subsequent gradient processing: + getSubPixel_gray(I_padded_neighborhood, old_image_buf, p_x[p], p_y[p], half_window_size + 1, subpixel_factor); + + // Also get the original-sized neighborhood + for (x = 1; x < padded_patch_size - 1; x++) { + for (y = 1; y < padded_patch_size - 1; y++) { + ix1 = (y * padded_patch_size + x); + ix2 = ((y - 1) * patch_size + (x - 1)); + I_neighborhood[ix2] = I_padded_neighborhood[ix1]; + } + } + + // (2) get the x- and y- gradients + getGradientPatch(I_padded_neighborhood, DX, DY, half_window_size); + + // (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window + error = calculateG(G, DX, DY, half_window_size); + if (error == NO_MEMORY) { return NO_MEMORY; } + + for (it = 0; it < 4; it++) { + // printf("G[%d] = %d\n\r", it, G[it]); + G[it] /= 255; // to keep values in range + // printf("G[%d] = %d\n\r", it, G[it]); + } + // calculate G's determinant: + Det = G[0] * G[3] - G[1] * G[2]; + //printf("Det = %d\n\r", Det); + Det = Det / subpixel_factor; // so that the steps will be expressed in subpixel units + //printf("Det = %d\n\r", Det); + if (Det < 1) { + status[p] = 0; +// printf("irrevertible G\n"); + } + + // (4) iterate over taking steps in the image to minimize the error: + it = 0; + step_threshold = 2; // 0.2 as smallest step (L1) + v_x = 0; + v_y = 0; + step_x = step_threshold + 1; + step_y = step_threshold + 1; + + while (status[p] == 1 && it < max_iterations && (abs(step_x) >= step_threshold || abs(step_y) >= step_threshold)) { + //printf("it = %d, (p_x+v_x,p_y+v_y) = (%d,%d)\n\r", it, p_x[p]+v_x, p_y[p]+v_y); + //printf("it = %d;", it); + // if the pixel goes outside the ROI in the image, stop tracking: + if (!(p_x[p] + v_x > ((half_window_size + 1) * subpixel_factor) && p_x[p] + v_x < ((int)IMG_WIDTH - half_window_size) * subpixel_factor && p_y[p] + v_y > ((half_window_size + 1) * subpixel_factor) && p_y[p] + v_y < ((int)IMG_HEIGHT - half_window_size)*subpixel_factor)) { +// printf("Outside of ROI, P1[%d,%d]\n\r",p_x[p],p_y[p]); + status[p] = 0; + break; + } + + // [a] get the subpixel neighborhood in the new image + // clear J: + for (x = 0; x < patch_size; x++) { + for (y = 0; y < patch_size; y++) { + ix2 = (y * patch_size + x); + J_neighborhood[ix2] = 0; + } + } + + + getSubPixel_gray(J_neighborhood, new_image_buf, p_x[p] + v_x, p_y[p] + v_y, half_window_size, subpixel_factor); + // [b] determine the image difference between the two neighborhoods + //printf("I = "); + //printIntMatrix(I_neighborhood, patch_size, patch_size); + //printf("J = "); + //printIntMatrix(J_neighborhood, patch_size, patch_size); + //getSubPixel(J_neighborhood, new_image_buf, subpixel_factor * ((p_x[p]+v_x)/subpixel_factor), subpixel_factor * ((p_y[p]+v_y) / subpixel_factor), half_window_size, subpixel_factor); + //printf("J2 = "); + //printIntMatrix(J_neighborhood, patch_size, patch_size); + //printf("figure(); subplot(1,2,1); imshow(I/255); subplot(1,2,2); imshow(J/255);\n\r"); + getImageDifference(I_neighborhood, J_neighborhood, ImDiff, patch_size, patch_size); + //printf("ImDiff = "); + //printIntMatrix(ImDiff, patch_size, patch_size); + error = calculateError(ImDiff, patch_size, patch_size) / 255; + +// if(error > error_threshold) printf("error threshold\n"); + if (error > error_threshold && it > max_iterations / 2) { + status[p] = 0; +// printf("occlusion\n"); + break; + } + //printf("error(%d) = %d;\n\r", it+1, error); + // [c] calculate the 'b'-vector + //printf("DX = "); + //printIntMatrix(DX, patch_size, patch_size); + multiplyImages(ImDiff, DX, IDDX, patch_size, patch_size); + //printf("IDDX = "); + //printIntMatrix(IDDX, patch_size, patch_size); + multiplyImages(ImDiff, DY, IDDY, patch_size, patch_size); + //printf("DY = "); + //printIntMatrix(DY, patch_size, patch_size); + //printf("IDDY = "); + //printIntMatrix(IDDY, patch_size, patch_size); + //printf("figure(); subplot(2,3,1); imagesc(ImDiff); subplot(2,3,2); imagesc(DX); subplot(2,3,3); imagesc(DY);"); + //printf("subplot(2,3,4); imagesc(IDDY); subplot(2,3,5); imagesc(IDDX);\n\r"); + // division by 255 to keep values in range: + b_x = getSumPatch(IDDX, patch_size) / 255; + b_y = getSumPatch(IDDY, patch_size) / 255; + //printf("b_x = %d; b_y = %d;\n\r", b_x, b_y); + // [d] calculate the additional flow step and possibly terminate the iteration + step_x = (G[3] * b_x - G[1] * b_y) / Det; + step_y = (G[0] * b_y - G[2] * b_x) / Det; + v_x += step_x; + v_y += step_y; // - (?) since the origin in the image is in the top left of the image, with y positive pointing down + //printf("step = [%d,%d]; v = [%d,%d];\n\r", step_x, step_y, v_x, v_y); + //printf("pause(0.5);\n\r"); + // next iteration + it++; + // step_size = abs(step_x); + // step_size += abs(step_y); + //printf("status = %d, it = %d, step_size = %d\n\r", status[p], it, step_size); + } // iteration to find the right window in the new image + + //printf("figure(); plot(error(1:(it+1)));\n\r"); +// printf("it = %d\n",it); + new_x[p] = (p_x[p] + v_x) / subpixel_factor; + new_y[p] = (p_y[p] + v_y) / subpixel_factor; + p_x[p] /= subpixel_factor; + p_y[p] /= subpixel_factor; + } + + + + // free all allocated variables: + free((int *) I_padded_neighborhood); + free((int *) I_neighborhood); + free((int *) J_neighborhood); + free((int *) DX); + free((int *) DY); + free((int *) ImDiff); + free((int *) IDDX); + free((int *) IDDY); + // no errors: + return OK; +} + +void quick_sort(float *a, int n) +{ + if (n < 2) { + return; + } + float p = a[n / 2]; + float *l = a; + float *r = a + n - 1; + while (l <= r) { + if (*l < p) { + l++; + continue; + } + if (*r > p) { + r--; + continue; // we need to check the condition (l <= r) every time we change the value of l or r + } + float t = *l; + *l++ = *r; + *r-- = t; + } + quick_sort(a, r - a + 1); + quick_sort(l, a + n - l); +} + +void quick_sort_int(int *a, int n) +{ + if (n < 2) { + return; + } + int p = a[n / 2]; + int *l = a; + int *r = a + n - 1; + while (l <= r) { + if (*l < p) { + l++; + continue; + } + if (*r > p) { + r--; + continue; + } + int t = *l; + *l++ = *r; + *r-- = t; + } + quick_sort_int(a, r - a + 1); + quick_sort_int(l, a + n - l); +} + +void CvtYUYV2Gray(unsigned char *grayframe, unsigned char *frame, int imW, int imH) +{ + int x, y; + unsigned char *Y, *gray; + for (y = 0; y < imH; y++) { + Y = frame + (imW * 2 * y) + 1; + gray = grayframe + (imW * y); + for (x = 0; x < imW; x += 2) { + gray[x] = *Y; + Y += 2; + gray[x + 1] = *Y; + Y += 2; + } + } +} + +unsigned int OF_buf_point = 0; +unsigned int OF_buf_point2 = 0; +float x_avg, y_avg, x_buf[24], y_buf[24], x_buf2[24], y_buf2[24]; + +void OFfilter(float *OFx, float *OFy, float dx, float dy, int count, int OF_FilterType) +{ + + if (OF_FilterType == 1) { //1. moving average 2. moving median + + x_avg = 0.0; + y_avg = 0.0; + + if (count) { + x_buf[OF_buf_point] = dx; + y_buf[OF_buf_point] = dy; + } else { + x_buf[OF_buf_point] = 0.0; + y_buf[OF_buf_point] = 0.0; + } + OF_buf_point = (OF_buf_point + 1) % 20; + + for (int i = 0; i < 20; i++) { + x_avg += x_buf[i] * 0.05; + y_avg += y_buf[i] * 0.05; + } + + *OFx = x_avg; + *OFy = y_avg; + + } else if (OF_FilterType == 2) { + if (count) { + x_buf2[OF_buf_point2] = dx; + y_buf2[OF_buf_point2] = dy; + } else { + x_buf2[OF_buf_point2] = 0.0; + y_buf2[OF_buf_point2] = 0.0; + } + OF_buf_point2 = (OF_buf_point2 + 1) % 11; // 11 + + quick_sort(x_buf2, 11); // 11 + quick_sort(y_buf2, 11); // 11 + + *OFx = x_buf2[6]; // 6 + *OFy = y_buf2[6]; // 6 + } else { + printf("no filter type selected!\n"); + } +} + diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h new file mode 100644 index 00000000000..14cc568efe5 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h @@ -0,0 +1,43 @@ +/* + * Copyright (C) 2014 Hann Woei Ho + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * @file paparazzi/sw/ext/ardrone2_vision/cv/opticflow/optic_flow_gdc.h + * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * + * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 + */ + +#ifndef OPTIC +#define OPTIC +void multiplyImages(int *ImA, int *ImB, int *ImC, int width, int height); +void getImageDifference(int *ImA, int *ImB, int *ImC, int width, int height); +void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int center_y, int half_window_size, int subpixel_factor); +void getGradientPatch(int *Patch, int *DX, int *DY, int half_window_size); +int getSumPatch(int *Patch, int size); +int calculateG(int *G, int *DX, int *DY, int half_window_size); +int calculateError(int *ImC, int width, int height); +int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int *p_x, int *p_y, int n_found_points, int imW, int imH, int *new_x, int *new_y, int *status, int half_window_size, int max_iterations); +void quick_sort(float *a, int n); +void quick_sort_int(int *a, int n); +void CvtYUYV2Gray(unsigned char *grayframe, unsigned char *frame, int imW, int imH); +void OFfilter(float *OFx, float *OFy, float dx, float dy, int count, int OF_FilterType); +#endif diff --git a/sw/airborne/modules/computer_vision/cv/resize.h b/sw/airborne/modules/computer_vision/cv/resize.h index e2bbf7096ff..22f06888cf6 100644 --- a/sw/airborne/modules/computer_vision/cv/resize.h +++ b/sw/airborne/modules/computer_vision/cv/resize.h @@ -36,13 +36,13 @@ inline void resize_uyuv(struct img_struct *input, struct img_struct *output, int *dest++ = *source++; // U *dest++ = *source++; // Y // now skip 3 pixels - if (pixelskip) { source += (pixelskip + 1) * 2; } + source += (pixelskip + 1) * 2; *dest++ = *source++; // U *dest++ = *source++; // V - if (pixelskip) { source += (pixelskip - 1) * 2; } + source += (pixelskip - 1) * 2; } // skip 3 rows - if (pixelskip) { source += pixelskip * input->w * 2; } + source += pixelskip * input->w * 2; } } diff --git a/sw/airborne/modules/computer_vision/cv/trig.c b/sw/airborne/modules/computer_vision/cv/trig.c new file mode 100644 index 00000000000..47110a216ff --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/trig.c @@ -0,0 +1,158 @@ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * Trigonometry + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ + +#include "trig.h" + +static int cosine[] = { + 10000, 9998, 9994, 9986, 9976, 9962, 9945, 9925, 9903, 9877, + 9848, 9816, 9781, 9744, 9703, 9659, 9613, 9563, 9511, 9455, + 9397, 9336, 9272, 9205, 9135, 9063, 8988, 8910, 8829, 8746, + 8660, 8572, 8480, 8387, 8290, 8192, 8090, 7986, 7880, 7771, + 7660, 7547, 7431, 7314, 7193, 7071, 6947, 6820, 6691, 6561, + 6428, 6293, 6157, 6018, 5878, 5736, 5592, 5446, 5299, 5150, + 5000, 4848, 4695, 4540, 4384, 4226, 4067, 3907, 3746, 3584, + 3420, 3256, 3090, 2924, 2756, 2588, 2419, 2250, 2079, 1908, + 1736, 1564, 1392, 1219, 1045, 872, 698, 523, 349, 175, + 0 +}; + +int sin_zelf(int ix) +{ + while (ix < 0) { + ix = ix + 360; + } + while (ix >= 360) { + ix = ix - 360; + } + if (ix < 90) { return cosine[90 - ix] / 10; } + if (ix < 180) { return cosine[ix - 90] / 10; } + if (ix < 270) { return -cosine[270 - ix] / 10; } + if (ix < 360) { return -cosine[ix - 270] / 10; } + return 0; +} + +int cos_zelf(int ix) +{ + while (ix < 0) { + ix = ix + 360; + } + while (ix >= 360) { + ix = ix - 360; + } + if (ix < 90) { return cosine[ix] / 10; } + if (ix < 180) { return -cosine[180 - ix] / 10; } + if (ix < 270) { return -cosine[ix - 180] / 10; } + if (ix < 360) { return cosine[360 - ix] / 10; } + return 0; +} + +int tan_zelf(int ix) +{ + + while (ix < 0) { + ix = ix + 360; + } + while (ix >= 360) { + ix = ix - 360; + } + if (ix == 90) { return 9999; } + if (ix == 270) { return -9999; } + if (ix < 90) { return (1000 * cosine[90 - ix]) / cosine[ix]; } + if (ix < 180) { return -(1000 * cosine[ix - 90]) / cosine[180 - ix]; } + if (ix < 270) { return (1000 * cosine[270 - ix]) / cosine[ix - 180]; } + if (ix < 360) { return -(1000 * cosine[ix - 270]) / cosine[360 - ix]; } + return 0; +} + +int asin_zelf(int y, int hyp) +{ + int quot, sgn, ix; + if ((y > hyp) || (y == 0)) { + return 0; + } + sgn = hyp * y; + if (hyp < 0) { + hyp = -hyp; + } + if (y < 0) { + y = -y; + } + quot = (y * 10000) / hyp; + if (quot > 9999) { + quot = 9999; + } + for (ix = 0; ix < 90; ix++) + if ((quot < cosine[ix]) && (quot >= cosine[ix + 1])) { + break; + } + if (sgn < 0) { + return -(90 - ix); + } else { + return 90 - ix; + } +} + +int acos_zelf(int x, int hyp) +{ + int quot, sgn, ix; + if (x > hyp) { + return 0; + } + if (x == 0) { + if (hyp < 0) { + return -90; + } else { + return 90; + } + return 0; + } + sgn = hyp * x; + if (hyp < 0) { + hyp = -hyp; + } + if (x < 0) { + x = -x; + } + quot = (x * 10000) / hyp; + if (quot > 9999) { + quot = 9999; + } + for (ix = 0; ix < 90; ix++) + if ((quot < cosine[ix]) && (quot >= cosine[ix + 1])) { + break; + } + if (sgn < 0) { + return -ix; + } else { + return ix; + } +} + +//atan_zelf(y/x) in degrees +int atan_zelf(int y, int x) +{ + int angle, flip, t, xy; + + if (x < 0) { x = -x; } + if (y < 0) { y = -y; } + flip = 0; if (x < y) { flip = 1; t = x; x = y; y = t; } + if (x == 0) { return 90; } + + xy = (y * 1000) / x; + angle = (360 * xy) / (6283 + ((((1764 * xy) / 1000) * xy) / 1000)); + if (flip) { angle = 90 - angle; } + return angle; +} + +unsigned int isqrt(unsigned int val) +{ + unsigned int temp, g = 0, b = 0x8000, bshft = 15; + do { + if (val >= (temp = (((g << 1) + b) << bshft--))) { + g += b; + val -= temp; + } + } while (b >>= 1); + return g; +} diff --git a/sw/airborne/modules/computer_vision/cv/trig.h b/sw/airborne/modules/computer_vision/cv/trig.h new file mode 100644 index 00000000000..9603d41789c --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/trig.h @@ -0,0 +1,7 @@ +int cos_zelf(int ix); +int sin_zelf(int); +int tan_zelf(int); +int acos_zelf(int, int); +int asin_zelf(int, int); +int atan_zelf(int, int); +unsigned int isqrt(unsigned int); diff --git a/sw/airborne/modules/computer_vision/lib/paparazzi.h b/sw/airborne/modules/computer_vision/lib/paparazzi.h new file mode 100644 index 00000000000..f3b2aef8f13 --- /dev/null +++ b/sw/airborne/modules/computer_vision/lib/paparazzi.h @@ -0,0 +1,32 @@ + +//////////////////////////////////////////////// +// Paparazzi communication interface + +#include // pthread_create +#include // gprint + +#include "socket.h" +#include "video_message_structs.h" + +struct gst2ppz_message_struct gst2ppz; +struct ppz2gst_message_struct ppz2gst; + +inline void paparazzi_message_server_start(void); +inline void paparazzi_message_send(void); + +struct UdpSocket *sock; + +inline void paparazzi_message_server_start(void) +{ + sock = udp_socket("192.168.1.1", 2000, 2001, FMS_UNICAST); +} + + +inline void paparazzi_message_send(void) +{ + udp_write(sock, (char *) &gst2ppz, sizeof(gst2ppz)); + int ret = udp_read(sock, (unsigned char *) &ppz2gst, sizeof(ppz2gst)); + printf("read: %d \n",ret); +} + + diff --git a/sw/airborne/modules/computer_vision/lib/tcp/socket.c b/sw/airborne/modules/computer_vision/lib/tcp/socket.c new file mode 100644 index 00000000000..9479c698fad --- /dev/null +++ b/sw/airborne/modules/computer_vision/lib/tcp/socket.c @@ -0,0 +1,139 @@ +#include "socket.h" +#include /* socket definitions */ +#include /* socket types */ +#include /* inet (3) funtions */ +#include /* misc. UNIX functions */ +#include +#include /* memset */ + +#include +#include + + + + +/* Global constants */ + +#define ECHO_PORT (2002) +#define MAX_LINE (1000) + +/* Global variables */ +int list_s; /* listening socket */ +int conn_s; /* connection socket */ +struct sockaddr_in servaddr; /* socket address structure */ +char buffer[MAX_LINE]; /* character buffer */ +char *endptr; /* for strtol() */ + +int closeSocket(void) +{ + return close(conn_s); +} + +int initSocket(void) +{ + + /* Create the listening socket */ + + if ((list_s = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + fprintf(stderr, "ECHOSERV: Error creating listening socket.\n"); + return -1; + } + + + /* Set all bytes in socket address structure to + zero, and fill in the relevant data members */ + + memset(&servaddr, 0, sizeof(servaddr)); + servaddr.sin_family = AF_INET; + servaddr.sin_addr.s_addr = htonl(INADDR_ANY); + servaddr.sin_port = htons(ECHO_PORT); + + /* Bind our socket addresss to the + listening socket, and call listen() */ + + if (bind(list_s, (struct sockaddr *) &servaddr, sizeof(servaddr)) < 0) { + fprintf(stderr, "ECHOSERV: Error calling bind()\n"); + exit(EXIT_FAILURE); + } + + if (listen(list_s, LISTENQ) < 0) { + fprintf(stderr, "ECHOSERV: Error calling listen()\n"); + exit(EXIT_FAILURE); + } + + + + /* Wait for a connection, then accept() it */ + if ((conn_s = accept(list_s, NULL, NULL)) < 0) { + fprintf(stderr, "ECHOSERV: Error calling function accept()\n"); + return -1; + } + printf("Connected!\n"); + return 1; +} + +/* Read a line from a socket */ + +ssize_t Readline_socket(void *vptr, size_t maxlen) +{ + ssize_t n, rc; + char c, *buffer; + + buffer = vptr; + + for (n = 1; n < maxlen; n++) { + + if ((rc = read(conn_s, &c, 1)) == 1) { + *buffer++ = c; + if (c == '\n') { + break; + } + } else if (rc == 0) { + if (n == 1) { + return 0; + } else { + break; + } + } else { + if (errno == EINTR) { + continue; + } + return -1; + } + } + + *buffer = 0; + return n; +} + + +/* Write a line to a socket */ + +ssize_t Writeline_socket(struct img_struct *img, size_t n) +{ + size_t nleft; + ssize_t nwritten; + unsigned char *buf1 = img->buf; + nleft = n; + + //printf("socket buf1 %d, %d, %d, %d, %d, %d\n",buf1[0],buf1[1],buf1[2],buf1[3],buf1[4],buf1[5]); + + while (nleft > 0) { + if ((nwritten = write(conn_s, buf1, nleft)) <= 0) { + if (errno == EINTR) { + nwritten = 0; + } else { + return -1; + } + } + nleft -= nwritten; + buf1 += nwritten; + } + + return n; + +} + + + + diff --git a/sw/airborne/modules/computer_vision/lib/tcp/socket.h b/sw/airborne/modules/computer_vision/lib/tcp/socket.h new file mode 100644 index 00000000000..04091cd4848 --- /dev/null +++ b/sw/airborne/modules/computer_vision/lib/tcp/socket.h @@ -0,0 +1,17 @@ +#ifndef SOCKET_H +#define SOCKET_H + +#include /* for ssize_t data type */ +#include "./video/video.h" + +#define LISTENQ (1024) /* Backlog for listen() */ + + +/* Function declarations */ +int initSocket(void) ; +ssize_t Readline(void *vptr, size_t maxlen); +ssize_t Writeline(struct img_struct *img, size_t maxlen); +int closeSocket(void); + +#endif /* SOCKET_H */ + diff --git a/sw/airborne/modules/computer_vision/lib/udp/socket.c b/sw/airborne/modules/computer_vision/lib/udp/socket.c index fba489e2828..34d73e2023b 100644 --- a/sw/airborne/modules/computer_vision/lib/udp/socket.c +++ b/sw/airborne/modules/computer_vision/lib/udp/socket.c @@ -22,8 +22,7 @@ # define SOCKET_ERROR -1 # define IO_SOCKET ioctl -struct UdpSocket *udp_socket(const char *str_ip_out, const int port_out, const int port_in, const int broadcast) -{ +struct UdpSocket *udp_socket(const char *str_ip_out, const int port_out, const int port_in, const int broadcast) { struct UdpSocket *me = malloc(sizeof(struct UdpSocket)); diff --git a/sw/airborne/modules/computer_vision/lib/v4l/ardrone2.c b/sw/airborne/modules/computer_vision/lib/v4l/ardrone2.c new file mode 100644 index 00000000000..e69de29bb2d diff --git a/sw/airborne/modules/computer_vision/lib/v4l/ardrone2.h b/sw/airborne/modules/computer_vision/lib/v4l/ardrone2.h new file mode 100644 index 00000000000..e69de29bb2d diff --git a/sw/airborne/modules/computer_vision/lib/v4l/video.c b/sw/airborne/modules/computer_vision/lib/v4l/video.c index e0a9fb5c318..b3a090ced43 100644 --- a/sw/airborne/modules/computer_vision/lib/v4l/video.c +++ b/sw/airborne/modules/computer_vision/lib/v4l/video.c @@ -43,10 +43,10 @@ pthread_t video_thread; -void *video_thread_main(void *data); -void *video_thread_main(void *data) +void *video_thread_main(void* data); +void *video_thread_main(void* data) { - struct vid_struct *vid = (struct vid_struct *)data; + struct vid_struct* vid = (struct vid_struct*)data; printf("video_thread_main started\n"); while (1) { fd_set fds; @@ -61,7 +61,7 @@ void *video_thread_main(void *data) r = select(vid->fd + 1, &fds, NULL, NULL, &tv); if (-1 == r) { - if (EINTR == errno) { continue; } + if (EINTR == errno) continue; printf("select err\n"); } @@ -84,13 +84,13 @@ void *video_thread_main(void *data) vid->seq++; - if (vid->trigger) { + if(vid->trigger) { // todo add timestamp again //vid->img->timestamp = util_timestamp(); vid->img->seq = vid->seq; - memcpy(vid->img->buf, vid->buffers[buf.index].buf, vid->w * vid->h * 2); - vid->trigger = 0; + memcpy(vid->img->buf, vid->buffers[buf.index].buf, vid->w*vid->h*2); + vid->trigger=0; } if (ioctl(vid->fd, VIDIOC_QBUF, &buf) < 0) { @@ -108,9 +108,9 @@ int video_init(struct vid_struct *vid) unsigned int i; enum v4l2_buf_type type; - vid->seq = 0; - vid->trigger = 0; - if (vid->n_buffers == 0) { vid->n_buffers = 4; } + vid->seq=0; + vid->trigger=0; + if(vid->n_buffers==0) vid->n_buffers=4; vid->fd = open(vid->device, O_RDWR | O_NONBLOCK, 0); @@ -148,7 +148,7 @@ int video_init(struct vid_struct *vid) printf("Buffer count = %d\n", vid->n_buffers); - vid->buffers = (struct buffer_struct *)calloc(vid->n_buffers, sizeof(struct buffer_struct)); + vid->buffers = (struct buffer_struct*)calloc(vid->n_buffers, sizeof(struct buffer_struct)); for (i = 0; i < vid->n_buffers; ++i) { struct v4l2_buffer buf; @@ -164,11 +164,11 @@ int video_init(struct vid_struct *vid) } vid->buffers[i].length = buf.length; - printf("buffer%d.length=%d\n", i, buf.length); - vid->buffers[i].buf = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, vid->fd, buf.m.offset); + printf("buffer%d.length=%d\n",i,buf.length); + vid->buffers[i].buf = mmap(NULL, buf.length, PROT_READ|PROT_WRITE, MAP_SHARED, vid->fd, buf.m.offset); if (MAP_FAILED == vid->buffers[i].buf) { - printf("mmap() failed.\n"); + printf ("mmap() failed.\n"); return -1; } } @@ -188,14 +188,14 @@ int video_init(struct vid_struct *vid) } type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - if (ioctl(vid->fd, VIDIOC_STREAMON, &type) < 0) { + if (ioctl(vid->fd, VIDIOC_STREAMON, &type)< 0) { printf("ioctl() VIDIOC_STREAMON failed.\n"); return -1; } //start video thread int rc = pthread_create(&video_thread, NULL, video_thread_main, vid); - if (rc) { + if(rc) { printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc); return 202; } @@ -207,28 +207,27 @@ void video_close(struct vid_struct *vid) { int i; for (i = 0; i < (int)vid->n_buffers; ++i) { - if (-1 == munmap(vid->buffers[i].buf, vid->buffers[i].length)) { printf("munmap() failed.\n"); } + if (-1 == munmap(vid->buffers[i].buf, vid->buffers[i].length)) printf("munmap() failed.\n"); } close(vid->fd); } struct img_struct *video_create_image(struct vid_struct *vid) { - struct img_struct *img = (struct img_struct *)malloc(sizeof(struct img_struct)); - img->w = vid->w; - img->h = vid->h; - img->buf = (unsigned char *)malloc(vid->h * vid->w * 2); + struct img_struct* img = (struct img_struct*)malloc(sizeof(struct img_struct)); + img->w=vid->w; + img->h=vid->h; + img->buf = (unsigned char*)malloc(vid->h*vid->w*2); return img; } pthread_mutex_t video_grab_mutex = PTHREAD_MUTEX_INITIALIZER; -void video_grab_image(struct vid_struct *vid, struct img_struct *img) -{ +void video_grab_image(struct vid_struct *vid, struct img_struct *img) { pthread_mutex_lock(&video_grab_mutex); vid->img = img; - vid->trigger = 1; + vid->trigger=1; // while(vid->trigger) pthread_yield(); - while (vid->trigger) { usleep(1); } + while(vid->trigger) usleep(1); pthread_mutex_unlock(&video_grab_mutex); } diff --git a/sw/airborne/modules/computer_vision/lib/v4l/video.h b/sw/airborne/modules/computer_vision/lib/v4l/video.h index 351ba68522e..125892ad1b9 100644 --- a/sw/airborne/modules/computer_vision/lib/v4l/video.h +++ b/sw/airborne/modules/computer_vision/lib/v4l/video.h @@ -25,7 +25,7 @@ #include "../../cv/image.h" struct buffer_struct { - void *buf; + void * buf; size_t length; }; @@ -39,7 +39,7 @@ struct vid_struct { //private members int trigger; struct img_struct *img; - struct buffer_struct *buffers; + struct buffer_struct * buffers; int fd; };