diff --git a/conf/airframes/ardrone2_opticflow_hover.xml b/conf/airframes/ardrone2_opticflow_hover.xml new file mode 100644 index 00000000000..fa1f9810e0e --- /dev/null +++ b/conf/airframes/ardrone2_opticflow_hover.xml @@ -0,0 +1,230 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + +
+ + + + + + + + + +
+ + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ + + + + + +
+ +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + + +
+ +
+ + + +
+ +
+ + + + +
+ +
+ + + + + +
+ diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml index 4feeb1d97af..1be7679ffd6 100644 --- a/conf/conf_tests.xml +++ b/conf/conf_tests.xml @@ -318,6 +318,17 @@ settings_modules="modules/meteo_france_DAQ.xml" gui_color="blue" /> + - - + + + + + + + + + + + + + + + + + + + + + + + + @@ -1997,7 +2019,7 @@ - + diff --git a/conf/modules/ctrl_module_demo.xml b/conf/modules/ctrl_module_demo.xml new file mode 100644 index 00000000000..9a322330058 --- /dev/null +++ b/conf/modules/ctrl_module_demo.xml @@ -0,0 +1,30 @@ + + + + + + Demo Control Module + + Rate-controller as module sample + + + + + + + + + + + + +
+ +
+ + + + + + + diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml new file mode 100644 index 00000000000..af3f680a58e --- /dev/null +++ b/conf/modules/cv_opticflow.xml @@ -0,0 +1,75 @@ + + + + + + Compute Optic Flow from Ardrone2 Bottom Camera + + Computes Pitch- and rollrate corrected optic flow from downward looking + ARDrone2 camera looking at a textured floor. + + - Sonar is required. + - Controller can hold position + + +
+ + + + + + + +
+ + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index e55f24fe6df..9795b058027 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -423,6 +423,11 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) case AP_MODE_NAV: guidance_h_mode_changed(GUIDANCE_H_MODE_NAV); break; + case AP_MODE_MODULE: +#ifdef GUIDANCE_H_MODE_MODULE_SETTING + guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_SETTING); +#endif + break; default: break; } @@ -464,6 +469,11 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) case AP_MODE_NAV: guidance_v_mode_changed(GUIDANCE_V_MODE_NAV); break; + case AP_MODE_MODULE: +#ifdef GUIDANCE_V_MODE_MODULE_SETTING + guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE_SETTING); +#endif + break; default: break; } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index 75a52806e74..8ee77b2afff 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot.h @@ -50,6 +50,7 @@ #define AP_MODE_RC_DIRECT 14 // Safety Pilot Direct Commands for helicopter direct control #define AP_MODE_CARE_FREE_DIRECT 15 #define AP_MODE_FORWARD 16 +#define AP_MODE_MODULE 17 extern uint8_t autopilot_mode; extern uint8_t autopilot_mode_auto2; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index bfed1160245..0737340fe87 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -27,6 +27,7 @@ #include "generated/airframe.h" #include "firmwares/rotorcraft/guidance/guidance_h.h" +#include "firmwares/rotorcraft/guidance/guidance_module.h" #include "firmwares/rotorcraft/stabilization.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" #include "firmwares/rotorcraft/navigation.h" @@ -252,6 +253,12 @@ void guidance_h_mode_changed(uint8_t new_mode) stabilization_attitude_enter(); break; +#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE + case GUIDANCE_H_MODE_MODULE: + guidance_h_module_enter(); + break; +#endif + case GUIDANCE_H_MODE_NAV: guidance_h_nav_enter(); #if NO_ATTITUDE_RESET_ON_MODE_CHANGE @@ -304,6 +311,12 @@ void guidance_h_read_rc(bool_t in_flight) #endif break; +#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE + case GUIDANCE_H_MODE_MODULE: + guidance_h_module_read_rc(); + break; +#endif + case GUIDANCE_H_MODE_NAV: if (radio_control.status == RC_OK) { stabilization_attitude_read_rc_setpoint_eulers(&guidance_h_rc_sp, in_flight, FALSE, FALSE); @@ -387,6 +400,12 @@ void guidance_h_run(bool_t in_flight) stabilization_attitude_run(in_flight); break; +#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE + case GUIDANCE_H_MODE_MODULE: + guidance_h_module_run(in_flight); + break; +#endif + default: break; } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index ac9b0640172..626ae4a7e86 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -57,6 +57,7 @@ #define GUIDANCE_H_MODE_RC_DIRECT 5 #define GUIDANCE_H_MODE_CARE_FREE 6 #define GUIDANCE_H_MODE_FORWARD 7 +#define GUIDANCE_H_MODE_MODULE 8 extern uint8_t guidance_h_mode; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h new file mode 100644 index 00000000000..f0d7cf8aa49 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2015 + * + * 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 firmwares/rotorcraft/guidance/guidance_module.h + * Guidance in a module file. + * + * Implement a custom controller in a module. + * Re-use desired modes: + * + * e.g.: #define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_HOVER + * can be used to only define a horizontal control in the module and use normal z_hold + * + * The guidance that the module implement must be activated with following defines: + * + * a) Implement own Horizontal loops when GUIDANCE_H_MODE_MODULE_SETTING is set to GUIDANCE_H_MODE_MODULE + * One must then implement: + * - void guidance_h_module_enter(void); + * - void guidance_h_module_read_rc(void); + * - void guidance_h_module_run(bool_t in_flight); + * + * + * b) Implement own Vertical loops when GUIDANCE_V_MODE_MODULE_SETTING is set to GUIDANCE_V_MODE_MODULE + * - void guidance_v_module_enter(void); + * - void guidance_v_module_run(bool_t in_flight); + * + * If the module implements both V and H mode, take into account that the H is called first and later V + * + */ + +#ifndef GUIDANCE_MODULE_H_ +#define GUIDANCE_MODULE_H_ + +#include "generated/modules.h" + + +#endif /* GUIDANCE_MODULE_H_ */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index ec8ab3082f7..c6d74dae38e 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -26,6 +26,7 @@ #include "generated/airframe.h" #include "firmwares/rotorcraft/guidance/guidance_v.h" +#include "firmwares/rotorcraft/guidance/guidance_module.h" #include "subsystems/radio_control.h" #include "firmwares/rotorcraft/stabilization.h" @@ -237,6 +238,12 @@ void guidance_v_mode_changed(uint8_t new_mode) GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0); break; +#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE + case GUIDANCE_V_MODE_MODULE: + guidance_v_module_enter(); + break; +#endif + default: break; @@ -307,6 +314,12 @@ void guidance_v_run(bool_t in_flight) stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; break; +#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE + case GUIDANCE_V_MODE_MODULE: + guidance_v_module_run(in_flight); + break; +#endif + case GUIDANCE_V_MODE_NAV: { if (vertical_mode == VERTICAL_MODE_ALT) { guidance_v_z_sp = -nav_flight_altitude; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h index ee836ec83ef..49053be7fd7 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h @@ -38,6 +38,7 @@ #define GUIDANCE_V_MODE_CLIMB 3 #define GUIDANCE_V_MODE_HOVER 4 #define GUIDANCE_V_MODE_NAV 5 +#define GUIDANCE_V_MODE_MODULE 6 extern uint8_t guidance_v_mode; diff --git a/sw/airborne/modules/computer_vision/cv/framerate.c b/sw/airborne/modules/computer_vision/cv/framerate.c new file mode 100644 index 00000000000..9ec262d0bcc --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/framerate.c @@ -0,0 +1,76 @@ +/* + * Copyright (C) 2015 The Paparazzi Community + * + * 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, see + * . + */ + +/** + * @file modules/computer_vision/cv/framerate.c + * + */ + +#include "std.h" +#include "framerate.h" + +// Frame Rate (FPS) +#include + +// local variables +volatile long timestamp; +struct timeval start_time; +struct timeval end_time; + +#define USEC_PER_SEC 1000000L + +static 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; +} + +static void start_timer(void) +{ + gettimeofday(&start_time, NULL); +} + +static long end_timer(void) +{ + gettimeofday(&end_time, NULL); + return time_elapsed(&start_time, &end_time); +} + + +void framerate_init(void) { + // Frame Rate Initialization + timestamp = 0; + start_timer(); +} + +float framerate_run(void) { + // FPS + timestamp = end_timer(); + float framerate_FPS = (float) 1000000 / (float)timestamp; + // printf("dt = %d, FPS = %f\n",timestamp, FPS); + start_timer(); + return framerate_FPS; +} diff --git a/sw/airborne/modules/computer_vision/cv/framerate.h b/sw/airborne/modules/computer_vision/cv/framerate.h new file mode 100644 index 00000000000..9044881ff05 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/framerate.h @@ -0,0 +1,28 @@ +/* + * Copyright (C) 2015 The Paparazzi Community + * + * 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, see + * . + */ + +/** + * @file modules/computer_vision/cv/framerate.h + * + */ + + +void framerate_init(void); +float framerate_run(void); 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..63a85126e57 --- /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..a8d856f9b37 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.c @@ -0,0 +1,7320 @@ +/* +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..683bee37c78 --- /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_int.c b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.c new file mode 100644 index 00000000000..b1e385972e5 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.c @@ -0,0 +1,523 @@ +/* + * Copyright (C) 2014 + * + * 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, see + * . + */ + +/** + * @file modules/computer_vision/cv/opticflow/optic_flow_int.c + * @brief efficient fixed-point optical-flow + * + * - Initial fixed-point C implementation by G. de Croon + * - Algorithm: Lucas-Kanade by Yves Bouguet + * - Publication: http://robots.stanford.edu/cs223b04/algo_tracking.pdf + */ + +#include +#include +#include +#include +#include "optic_flow_int.h" +#include "modules/computer_vision/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; + 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; + + 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 (x == x_0 && y == y_0) { + 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]; + 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]; + 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]; + 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]; + Patch[ix1] += alpha_x * alpha_y * ((int) Y); + + // normalize patch value + Patch[ix1] /= (subpixel_factor * subpixel_factor); + } + } + } + + 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) / 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) / 2; + + + } + } + + 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; + sum += Patch[ix]; // do not check thresholds + } + } + + 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++) { + // status: point is not yet lost: + status[p] = 1; + + // 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; + + // 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)) { + 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++) { + G[it] /= 255; // to keep values in range + } + // calculate G's determinant: + Det = G[0] * G[3] - G[1] * G[2]; + Det = Det / subpixel_factor; // so that the steps will be expressed in subpixel units + if (Det < 1) { + status[p] = 0; + } + + // (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)) { + // 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)) { + 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 + getImageDifference(I_neighborhood, J_neighborhood, ImDiff, patch_size, patch_size); + error = calculateError(ImDiff, patch_size, patch_size) / 255; + + if (error > error_threshold && it > max_iterations / 2) { + status[p] = 0; + break; + } + multiplyImages(ImDiff, DX, IDDX, patch_size, patch_size); + 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 + // next iteration + it++; + } // iteration to find the right window in the new image + + 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_int.h b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.h new file mode 100644 index 00000000000..786916ac625 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.h @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2014 + * + * 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, see + * . + */ + +/** + * @file modules/computer_vision/cv/opticflow/optic_flow_int.h + * @brief efficient fixed-point optical-flow + * + */ + +#ifndef OPTIC_FLOW_INT_H +#define OPTIC_FLOW_INT_H + +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 /* OPTIC_FLOW_INT_H */ diff --git a/sw/airborne/modules/computer_vision/cv/resize.h b/sw/airborne/modules/computer_vision/cv/resize.h index e2bbf7096ff..b0524a52705 100644 --- a/sw/airborne/modules/computer_vision/cv/resize.h +++ b/sw/airborne/modules/computer_vision/cv/resize.h @@ -23,26 +23,39 @@ #include #include "image.h" +/** Simplified high-speed low CPU downsample function without averaging + * + * downsample factor must be 1, 2, 4, 8 ... 2^X + * image of typ UYVY expected. Only one color UV per 2 pixels + * + * we keep the UV color of the first pixel pair + * and sample the intensity evenly 1-3-5-7-... or 1-5-9-... + * + * input: u1y1 v1y2 u3y3 v3y4 u5y5 v5y6 u7y7 v7y8 ... + * downsample=1 u1y1 v1y2 u3y3 v3y4 u5y5 v5y6 u7y7 v7y8 ... + * downsample=2 u1y1v1 (skip2) y3 (skip2) u5y5v5 (skip2 y7 (skip2) ... + * downsample=4 u1y1v1 (skip6) y5 (skip6) ... + */ + inline void resize_uyuv(struct img_struct *input, struct img_struct *output, int downsample); inline void resize_uyuv(struct img_struct *input, struct img_struct *output, int downsample) { uint8_t *source = input->buf; uint8_t *dest = output->buf; - int pixelskip = downsample - 1; + int pixelskip = (downsample - 1) * 2; for (int y = 0; y < output->h; y++) { for (int x = 0; x < output->w; x += 2) { // YUYV *dest++ = *source++; // U *dest++ = *source++; // Y - // now skip 3 pixels - if (pixelskip) { source += (pixelskip + 1) * 2; } - *dest++ = *source++; // U *dest++ = *source++; // V - if (pixelskip) { source += (pixelskip - 1) * 2; } + source += pixelskip; + *dest++ = *source++; // Y + source += pixelskip; } - // skip 3 rows - if (pixelskip) { source += pixelskip * input->w * 2; } + // read 1 in every 'downsample' rows, so skip (downsample-1) rows after reading the first + source += (downsample-1) * 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..88a1103f32b --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/trig.c @@ -0,0 +1,159 @@ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * 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/v4l/video.h b/sw/airborne/modules/computer_vision/lib/v4l/video.h index 351ba68522e..030f746d4b6 100644 --- a/sw/airborne/modules/computer_vision/lib/v4l/video.h +++ b/sw/airborne/modules/computer_vision/lib/v4l/video.h @@ -22,7 +22,7 @@ #ifndef _VIDEO_H #define _VIDEO_H -#include "../../cv/image.h" +#include "modules/computer_vision/cv/image.h" struct buffer_struct { void *buf; 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..8394ba32ec9 --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c @@ -0,0 +1,213 @@ +/* + * 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, see + * . + */ + +/** + * @file modules/computer_vision/opticflow/hover_stabilization.c + * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * + * Control loops for optic flow based hovering. + * Computes setpoint for the lower level attitude stabilization to control horizontal velocity. + */ + +// Own Header +#include "hover_stabilization.h" + +// Stabilization +#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h" +#include "firmwares/rotorcraft/guidance/guidance_v.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; + + +#ifndef VISION_HOVER +#define VISION_HOVER TRUE +#endif + +#ifndef VISION_PHI_PGAIN +#define VISION_PHI_PGAIN 500. +#endif + +#ifndef VISION_PHI_IGAIN +#define VISION_PHI_IGAIN 10. +#endif + +#ifndef VISION_THETA_PGAIN +#define VISION_THETA_PGAIN 500. +#endif + +#ifndef VISION_THETA_IGAIN +#define VISION_THETA_IGAIN 10. +#endif + +#ifndef VISION_DESIRED_VX +#define VISION_DESIRED_VX 0. +#endif + +#ifndef VISION_DESIRED_VY +#define VISION_DESIRED_VY 0. +#endif + +void run_opticflow_hover(void); + +void guidance_h_module_enter(void) +{ + // INIT + Velx_Int = 0; + Vely_Int = 0; + // GUIDANCE: Set Hover-z-hold + guidance_v_z_sp = -1; +} + +void guidance_h_module_read_rc(void) +{ + // Do not read RC + // Setpoint being set by vision +} + +void guidance_h_module_run(bool_t in_flight) +{ + // Run + // Setpoint being set by vision + stabilization_attitude_run(in_flight); +} + + +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(struct CVresults* vision ) +{ + struct FloatVect3 V_body; + if (activate_opticflow_hover == TRUE) { + // Compute body velocities from ENU + struct FloatVect3 *vel_ned = (struct FloatVect3*)stateGetSpeedNed_f(); + struct FloatQuat *q_n2b = stateGetNedToBodyQuat_f(); + float_quat_vmult(&V_body, q_n2b, vel_ned); + } + + // ************************************************************************************* + // Downlink Message + // ************************************************************************************* + + DOWNLINK_SEND_OF_HOVER(DefaultChannel, DefaultDevice, + &vision->FPS, &vision->dx_sum, &vision->dy_sum, &vision->OFx, &vision->OFy, + &vision->diff_roll, &vision->diff_pitch, + &vision->Velx, &vision->Vely, + &V_body.x, &V_body.y, + &vision->cam_h, &vision->count); + + if (autopilot_mode != AP_MODE_MODULE) { + return; + } + + if (vision->flow_count) { + Error_Velx = vision->Velx - vision_desired_vx; + Error_Vely = vision->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, &vision->Velx, &vision->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..285ac708649 --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h @@ -0,0 +1,60 @@ +/* + * 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, see + * . + */ + +/** + * @file modules/computer_vision/opticflow/hover_stabilization.h + * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * + * Control loops for optic flow based hovering. + * Computes setpoint for the lower level attitude stabilization to control horizontal velocity. + */ + +#ifndef HOVER_STABILIZATION_H_ +#define HOVER_STABILIZATION_H_ + +#include +#include "inter_thread_data.h" + +// Controller module + +// Vertical loop re-uses Alt-hold +#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_HOVER + +// Horizontal mode is a specific controller +#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE + +// Implement own Horizontal loops +extern void guidance_h_module_enter(void); +extern void guidance_h_module_read_rc(void); +extern void guidance_h_module_run(bool_t in_flight); + + +void init_hover_stabilization_onvision(void); +void run_hover_stabilization_onvision(struct CVresults *vision); + +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/inter_thread_data.h b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h new file mode 100644 index 00000000000..65ef0cb41e0 --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2015 The Paparazzi Community + * + * 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, see + * . + */ + +/** + * @file modules/computer_vision/opticflow/inter_thread_data.h + * @brief Inter-thread data structures. + * + * Data structures used to for inter-thread communication via Unix Domain sockets. + */ + + +#ifndef _INTER_THREAD_DATA_H +#define _INTER_THREAD_DATA_H + +/// Data from thread to module +struct CVresults { + int cnt; // Number of processed frames + + float Velx; // Velocity as measured by camera + float Vely; + int flow_count; + + float cam_h; // Debug parameters + int count; + float OFx, OFy, dx_sum, dy_sum; + float diff_roll; + float diff_pitch; + float FPS; +}; + +/// Data from module to thread +struct PPRZinfo { + int cnt; // IMU msg counter + float phi; // roll [rad] + float theta; // pitch [rad] + float agl; // height above ground [m] +}; + +#endif diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c new file mode 100644 index 00000000000..0566279f1b8 --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c @@ -0,0 +1,150 @@ +/* + * Copyright (C) 2015 The Paparazzi Community + * + * 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, see + * . + */ + +/** + * @file modules/computer_vision/opticflow/opticflow_thread.c + * + */ + +// Sockets +#include +#include +#include +#include + +#include "opticflow_thread.h" + + +///////////////////////////////////////////////////////////////////////// +// COMPUTER VISION THREAD + +// Video +#include "v4l/video.h" +#include "resize.h" + +// Payload Code +#include "visual_estimator.h" + +// Downlink Video +//#define DOWNLINK_VIDEO 1 + +#ifdef DOWNLINK_VIDEO +#include "encoding/jpeg.h" +#include "encoding/rtp.h" +#endif + +#include +#define DEBUG_INFO(X, ...) ; + +static volatile enum{RUN,EXIT} computer_vision_thread_command = RUN; /** request to close: set to 1 */ + +void computervision_thread_request_exit(void) { + computer_vision_thread_command = EXIT; +} + +void *computervision_thread_main(void *args) +{ + int thread_socket = *(int *) args; + + // Local data in/out + struct CVresults vision_results; + struct PPRZinfo autopilot_data; + + // Status + computer_vision_thread_command = RUN; + + // Video Input + struct vid_struct vid; + /* On ARDrone2: + * video1 = front camera; video2 = bottom camera + */ + vid.device = (char *)"/dev/video2"; + vid.w = 320; + vid.h = 240; + vid.n_buffers = 4; + + if (video_init(&vid) < 0) { + printf("Error initialising video\n"); + return 0; + } + + // Video Grabbing + struct img_struct *img_new = video_create_image(&vid); + +#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 + opticflow_plugin_init(vid.w, vid.h, &vision_results); + + while (computer_vision_thread_command == RUN) { + + // Wait for a new frame + video_grab_image(&vid, img_new); + + // Get most recent State information + int bytes_read = sizeof(autopilot_data); + while (bytes_read == sizeof(autopilot_data)) + { + bytes_read = recv(thread_socket, &autopilot_data, sizeof(autopilot_data), MSG_DONTWAIT); + if (bytes_read != sizeof(autopilot_data)) { + if (bytes_read != -1) { + printf("[thread] Failed to read %d bytes PPRZ info from socket.\n",bytes_read); + } + } + } + DEBUG_INFO("[thread] Read # %d\n",autopilot_data.cnt); + + // Run Image Processing with image and data and get results + opticflow_plugin_run(img_new->buf, &autopilot_data, &vision_results); + + /* Send results to main */ + vision_results.cnt++; + int bytes_written = write(thread_socket, &vision_results, sizeof(vision_results)); + if (bytes_written != sizeof(vision_results)){ + perror("[thread] Failed to write to socket.\n"); + } + DEBUG_INFO("[thread] Write # %d, (bytes %d)\n",vision_results.cnt, bytes_written); + +#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); + send_rtp_frame(vsock, jpegbuf, size, small.w, small.h, 0, quality_factor, dri_header, 0); +#endif + } + + printf("Thread Closed\n"); + video_close(&vid); + return 0; +} diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h new file mode 100644 index 00000000000..5826da2229b --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h @@ -0,0 +1,33 @@ +/* + * Copyright (C) 2015 The Paparazzi Community + * + * 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, see + * . + */ + +/** + * @file modules/computer_vision/opticflow/opticflow_thread.h + * @brief computer vision thread + * + */ + +#ifndef OPTICFLOW_THREAD_H +#define OPTICFLOW_THREAD_H + +void *computervision_thread_main(void *args); /* computer vision thread: should be given a pointer to a socketpair as argument */ +void computervision_thread_request_exit(void); + +#endif diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c new file mode 100644 index 00000000000..48bd4ffd5d2 --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -0,0 +1,283 @@ +/* + * 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, see + * . + */ + +/** + * @file modules/computer_vision/opticflow/visual_estimator.c + * @brief Estimate velocity from optic flow. + * + * Using sensors from vertical camera and IMU of Parrot AR.Drone 2.0. + * + * Warning: all this code is called form the Vision-Thread: do not access any autopilot data in here. + */ + +#include "std.h" + +#include +#include +#include + +// Own Header +#include "visual_estimator.h" + +// Computer Vision +#include "opticflow/optic_flow_int.h" +#include "opticflow/fast9/fastRosten.h" + +// for FPS +#include "modules/computer_vision/cv/framerate.h" + + +// Local variables +struct visual_estimator_struct +{ + // Image size + unsigned int imgWidth; + unsigned int imgHeight; + + // Images + uint8_t *prev_frame; + uint8_t *gray_frame; + uint8_t *prev_gray_frame; + + // Initialization + int old_img_init; + + // Store previous + float prev_pitch; + float prev_roll; +} visual_estimator; + +// 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 +#define MAX_COUNT 100 + +// Flow Derotation +#define FLOW_DEROTATION + + +// Called by plugin +void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *results) +{ + // Initialize variables + visual_estimator.imgWidth = w; + visual_estimator.imgHeight = h; + + visual_estimator.gray_frame = (unsigned char *) calloc(w * h, sizeof(uint8_t)); + visual_estimator.prev_frame = (unsigned char *) calloc(w * h * 2, sizeof(uint8_t)); + visual_estimator.prev_gray_frame = (unsigned char *) calloc(w * h, sizeof(uint8_t)); + + visual_estimator.old_img_init = 1; + visual_estimator.prev_pitch = 0.0; + visual_estimator.prev_roll = 0.0; + + results->OFx = 0.0; + results->OFy = 0.0; + results->dx_sum = 0.0; + results->dy_sum = 0.0; + results->diff_roll = 0.0; + results->diff_pitch = 0.0; + results->cam_h = 0.0; + results->Velx = 0.0; + results->Vely = 0.0; + results->flow_count = 0; + results->cnt = 0; + results->count = 0; + + framerate_init(); +} + +void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CVresults *results) +{ + // Corner Tracking + // Working Variables + int max_count = 25; + int borderx = 24, bordery = 24; + int x[MAX_COUNT], y[MAX_COUNT]; + int new_x[MAX_COUNT], new_y[MAX_COUNT]; + int status[MAX_COUNT]; + int dx[MAX_COUNT], dy[MAX_COUNT]; + int w = visual_estimator.imgWidth; + int h = visual_estimator.imgHeight; + + // Framerate Measuring + results->FPS = framerate_run(); + + if (visual_estimator.old_img_init == 1) { + memcpy(visual_estimator.prev_frame, frame, w * h * 2); + CvtYUYV2Gray(visual_estimator.prev_gray_frame, visual_estimator.prev_frame, w, h); + visual_estimator.old_img_init = 0; + } + + // ************************************************************************************* + // Corner detection + // ************************************************************************************* + + // FAST corner detection + int fast_threshold = 20; + xyFAST *pnts_fast; + pnts_fast = fast9_detect((const byte *)visual_estimator.prev_gray_frame, w, h, w, + fast_threshold, &results->count); + if (results->count > MAX_COUNT) { results->count = MAX_COUNT; } + for (int i = 0; i < results->count; i++) { + x[i] = pnts_fast[i].x; + y[i] = pnts_fast[i].y; + } + free(pnts_fast); + + // Remove neighboring corners + const float min_distance = 3; + float min_distance2 = min_distance * min_distance; + int labelmin[MAX_COUNT]; + for (int i = 0; i < results->count; i++) { + for (int j = i + 1; j < results->count; j++) { + // distance squared: + float 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 = results->count; + for (int i = results->count - 1; i >= 0; i--) { + int remove_point = 0; + + if (labelmin[i]) { + remove_point = 1; + } + + if (remove_point) { + for (int c = i; c < count_fil - 1; c++) { + x[c] = x[c + 1]; + y[c] = y[c + 1]; + } + count_fil--; + } + } + + if (count_fil > max_count) { count_fil = max_count; } + results->count = count_fil; + + // ************************************************************************************* + // Corner Tracking + // ************************************************************************************* + CvtYUYV2Gray(visual_estimator.gray_frame, frame, w, h); + + opticFlowLK(visual_estimator.gray_frame, visual_estimator.prev_gray_frame, x, y, + count_fil, w, h, new_x, new_y, status, 5, 100); + + results->flow_count = count_fil; + for (int i = count_fil - 1; i >= 0; i--) { + int remove_point = 1; + + if (status[i] && !(new_x[i] < borderx || new_x[i] > (w - 1 - borderx) || + new_y[i] < bordery || new_y[i] > (h - 1 - bordery))) { + remove_point = 0; + } + + if (remove_point) { + for (int c = i; c < results->flow_count - 1; c++) { + x[c] = x[c + 1]; + y[c] = y[c + 1]; + new_x[c] = new_x[c + 1]; + new_y[c] = new_y[c + 1]; + } + results->flow_count--; + } + } + + results->dx_sum = 0.0; + results->dy_sum = 0.0; + + // Optical Flow Computation + for (int i = 0; i < results->flow_count; i++) { + dx[i] = new_x[i] - x[i]; + dy[i] = new_y[i] - y[i]; + } + + // Median Filter + if (results->flow_count) { + quick_sort_int(dx, results->flow_count); // 11 + quick_sort_int(dy, results->flow_count); // 11 + + results->dx_sum = (float) dx[results->flow_count / 2]; + results->dy_sum = (float) dy[results->flow_count / 2]; + } else { + results->dx_sum = 0.0; + results->dy_sum = 0.0; + } + + // Flow Derotation + results->diff_pitch = (info->theta - visual_estimator.prev_pitch) * h / FOV_H; + results->diff_roll = (info->phi - visual_estimator.prev_roll) * w / FOV_W; + visual_estimator.prev_pitch = info->theta; + visual_estimator.prev_roll = info->phi; + + float OFx_trans, OFy_trans; +#ifdef FLOW_DEROTATION + if (results->flow_count) { + OFx_trans = results->dx_sum - results->diff_roll; + OFy_trans = results->dy_sum - results->diff_pitch; + + if ((OFx_trans <= 0) != (results->dx_sum <= 0)) { + OFx_trans = 0; + OFy_trans = 0; + } + } else { + OFx_trans = results->dx_sum; + OFy_trans = results->dy_sum; + } +#else + OFx_trans = results->dx_sum; + OFy_trans = results->dy_sum; +#endif + + // Average Filter + OFfilter(&results->OFx, &results->OFy, OFx_trans, OFy_trans, results->flow_count, 1); + + // Velocity Computation + if (info->agl < 0.01) { + results->cam_h = 0.01; + } + else { + results->cam_h = info->agl; + } + + if (results->flow_count) { + results->Velx = results->OFy * results->cam_h * results->FPS / Fy_ARdrone + 0.05; + results->Vely = -results->OFx * results->cam_h * results->FPS / Fx_ARdrone - 0.1; + } else { + results->Velx = 0.0; + results->Vely = 0.0; + } + + // ************************************************************************************* + // Next Loop Preparation + // ************************************************************************************* + + memcpy(visual_estimator.prev_frame, frame, w * h * 2); + memcpy(visual_estimator.prev_gray_frame, visual_estimator.gray_frame, w * h); + +} diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h new file mode 100644 index 00000000000..f499ffd320a --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h @@ -0,0 +1,41 @@ +/* + * 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, see + * . + */ + +/** + * @file modules/computer_vision/opticflow/visual_estimator.h + * @brief Estimate velocity from optic flow. + * + * Using sensors from vertical camera and IMU of Parrot AR.Drone 2.0 + */ + +#ifndef VISUAL_ESTIMATOR_H +#define VISUAL_ESTIMATOR_H + +#include "inter_thread_data.h" + +/** + * Initialize visual estimator. + * @param w image width + * @param h image height + */ +void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *results); +void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CVresults* results); + +#endif /* VISUAL_ESTIMATOR_H */ diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c new file mode 100644 index 00000000000..c8805c9eca7 --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -0,0 +1,147 @@ +/* + * 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, see + * . + */ + +/** + * @file modules/computer_vision/opticflow_module.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 "opticflow_module.h" + +// Computervision Runs in a thread +#include "opticflow/opticflow_thread.h" +#include "opticflow/inter_thread_data.h" + +// Navigate Based On Vision, needed to call init/run_hover_stabilization_onvision +#include "opticflow/hover_stabilization.h" + +// Threaded computer vision +#include + +// Sockets +#include +#include +#include +#include +#include + +int cv_sockets[2]; + +// Paparazzi Data +#include "state.h" +#include "subsystems/abi.h" + +// Downlink +#include "subsystems/datalink/downlink.h" + + +struct PPRZinfo opticflow_module_data; + +/** height above ground level, from ABI + * Used for scale computation, negative value means invalid. + */ +/** default sonar/agl to use in opticflow visual_estimator */ +#ifndef OPTICFLOW_AGL_ID +#define OPTICFLOW_AGL_ID ABI_BROADCAST +#endif +abi_event agl_ev; +static void agl_cb(uint8_t sender_id, const float *distance); + +static void agl_cb(uint8_t sender_id __attribute__((unused)), const float *distance) +{ + if (*distance > 0) { + opticflow_module_data.agl = *distance; + } +} + +#define DEBUG_INFO(X, ...) ; + +void opticflow_module_init(void) +{ + // get AGL from sonar via ABI + AbiBindMsgAGL(OPTICFLOW_AGL_ID, &agl_ev, agl_cb); + + // Initialize local data + opticflow_module_data.cnt = 0; + opticflow_module_data.phi = 0; + opticflow_module_data.theta = 0; + opticflow_module_data.agl = 0; + + // Stabilization Code Initialization + init_hover_stabilization_onvision(); +} + + +void opticflow_module_run(void) +{ + // Send Updated data to thread + opticflow_module_data.cnt++; + opticflow_module_data.phi = stateGetNedToBodyEulers_f()->phi; + opticflow_module_data.theta = stateGetNedToBodyEulers_f()->theta; + int bytes_written = write(cv_sockets[0], &opticflow_module_data, sizeof(opticflow_module_data)); + if (bytes_written != sizeof(opticflow_module_data)){ + printf("[module] Failed to write to socket: written = %d, error=%d.\n",bytes_written, errno); + } + else { + DEBUG_INFO("[module] Write # %d (%d bytes)\n",opticflow_module_data.cnt, bytes_written); + } + + // Read Latest Vision Module Results + struct CVresults vision_results; + // Warning: if the vision runs faster than the module, you need to read multiple times + int bytes_read = recv(cv_sockets[0], &vision_results, sizeof(vision_results), MSG_DONTWAIT); + if (bytes_read != sizeof(vision_results)) { + if (bytes_read != -1) { + printf("[module] Failed to read %d bytes: CV results from socket errno=%d.\n",bytes_read, errno); + } + } else { + //////////////////////////////////////////// + // Module-Side Code + //////////////////////////////////////////// + DEBUG_INFO("[module] Read vision %d\n",vision_results.cnt); + run_hover_stabilization_onvision(&vision_results); + } +} + +void opticflow_module_start(void) +{ + pthread_t computervision_thread; + if (socketpair(AF_UNIX, SOCK_DGRAM, 0, cv_sockets) == 0) { + //////////////////////////////////////////// + // Thread-Side Code + //////////////////////////////////////////// + int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main, + &cv_sockets[1]); + if (rc) { + printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc); + } + } + else { + perror("Could not create socket.\n"); + } +} + +void opticflow_module_stop(void) +{ + computervision_thread_request_exit(); +} diff --git a/sw/airborne/modules/computer_vision/opticflow_module.h b/sw/airborne/modules/computer_vision/opticflow_module.h new file mode 100644 index 00000000000..098d0b15b3e --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow_module.h @@ -0,0 +1,39 @@ +/* + * 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, see + * . + */ + +/** + * @file modules/computer_vision/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_MODULE_H +#define OPTICFLOW_MODULE_H + +#include "std.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); + +#endif /* OPTICFLOW_MODULE_H */ diff --git a/sw/airborne/modules/ctrl/ctrl_module_demo.c b/sw/airborne/modules/ctrl/ctrl_module_demo.c new file mode 100644 index 00000000000..0e3182467ea --- /dev/null +++ b/sw/airborne/modules/ctrl/ctrl_module_demo.c @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2015 + * + * 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, see + * . + */ + +/** + * @file modules/ctrl/ctrl_module_demo.h + * @brief example empty controller + * + */ + +#include "modules/ctrl/ctrl_module_demo.h" +#include "state.h" +#include "subsystems/radio_control.h" +#include "firmwares/rotorcraft/stabilization.h" + +struct ctrl_module_demo_struct { + int rc_x; + int rc_y; + int rc_z; + int rc_t; + +} ctrl_module_demo; + +float ctrl_module_demo_pr_ff_gain = 0.2f; // Pitch/Roll +float ctrl_module_demo_pr_d_gain = 0.1f; +float ctrl_module_demo_y_ff_gain = 0.4f; // Yaw +float ctrl_module_demo_y_d_gain = 0.05f; + +void ctrl_module_init(void); +void ctrl_module_run(bool_t in_flight); + +void ctrl_module_init(void) +{ + ctrl_module_demo.rc_x = 0; + ctrl_module_demo.rc_y = 0; + ctrl_module_demo.rc_z = 0; + ctrl_module_demo.rc_t = 0; +} + +// Old-fashened rate control without reference model nor attitude +void ctrl_module_run(bool_t in_flight) +{ + if (!in_flight) { + // Reset integrators + stabilization_cmd[COMMAND_ROLL] = 0; + stabilization_cmd[COMMAND_PITCH] = 0; + stabilization_cmd[COMMAND_YAW] = 0; + stabilization_cmd[COMMAND_THRUST] = 0; + } else { + stabilization_cmd[COMMAND_ROLL] = ctrl_module_demo.rc_x * ctrl_module_demo_pr_ff_gain - stateGetBodyRates_i()->p * + ctrl_module_demo_pr_d_gain; + stabilization_cmd[COMMAND_PITCH] = ctrl_module_demo.rc_y * ctrl_module_demo_pr_ff_gain - stateGetBodyRates_i()->q * + ctrl_module_demo_pr_d_gain; + stabilization_cmd[COMMAND_YAW] = ctrl_module_demo.rc_z * ctrl_module_demo_y_ff_gain - stateGetBodyRates_i()->r * + ctrl_module_demo_y_d_gain; + stabilization_cmd[COMMAND_THRUST] = ctrl_module_demo.rc_t; + } +} + + + +//////////////////////////////////////////////////////////////////// +// Call our controller +// Implement own Horizontal loops +void guidance_h_module_enter(void) +{ + ctrl_module_init(); +} +void guidance_h_module_read_rc(void) +{ + // -MAX_PPRZ to MAX_PPRZ + ctrl_module_demo.rc_t = radio_control.values[RADIO_THROTTLE]; + ctrl_module_demo.rc_x = radio_control.values[RADIO_ROLL]; + ctrl_module_demo.rc_y = radio_control.values[RADIO_PITCH]; + ctrl_module_demo.rc_z = radio_control.values[RADIO_YAW]; +} + +void guidance_h_module_run(bool_t in_flight) +{ + // Call full inner-/outerloop / horizontal-/vertical controller: + ctrl_module_run(in_flight); +} + +// Implement own Horizontal loops +inline void guidance_v_module_enter(void) {} +inline void guidance_v_module_run(UNUSED bool_t in_flight) {} + + diff --git a/sw/airborne/modules/ctrl/ctrl_module_demo.h b/sw/airborne/modules/ctrl/ctrl_module_demo.h new file mode 100644 index 00000000000..07a99eef7b7 --- /dev/null +++ b/sw/airborne/modules/ctrl/ctrl_module_demo.h @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2015 + * + * 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, see + * . + */ + +/** + * @file modules/ctrl/ctrl_module_demo.c + * @brief example empty controller + * + */ + +#ifndef CTRL_MODULE_DEMO_H_ +#define CTRL_MODULE_DEMO_H_ + +#include + +// Settings +extern float ctrl_module_demo_pr_ff_gain; // Pitch/Roll +extern float ctrl_module_demo_pr_d_gain; +extern float ctrl_module_demo_y_ff_gain; // Yaw +extern float ctrl_module_demo_y_d_gain; + + +// Demo with own guidance_h +#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE + +// and own guidance_v +#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE + +// Implement own Horizontal loops +extern void guidance_h_module_enter(void); +extern void guidance_h_module_read_rc(void); +extern void guidance_h_module_run(bool_t in_flight); + +// Implement own Horizontal loops +extern void guidance_v_module_enter(void); +extern void guidance_v_module_run(bool_t in_flight); + + +#endif /* HOVER_STABILIZATION_H_ */ diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 9f89de3fd69..4a77d9b001e 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -1266,7 +1266,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> let color = match ap_mode with "AUTO2" | "NAV" -> ok_color - | "AUTO1" | "R_RCC" | "A_RCC" | "ATT_C" | "R_ZH" | "A_ZH" | "HOVER" | "HOV_C" | "H_ZH" -> "#10F0E0" + | "AUTO1" | "R_RCC" | "A_RCC" | "ATT_C" | "R_ZH" | "A_ZH" | "HOVER" | "HOV_C" | "H_ZH" | "MODULE" -> "#10F0E0" | "MANUAL" | "RATE" | "ATT" | "RC_D" | "CF" | "FWD" -> warning_color | _ -> alert_color in ac.strip#set_color "AP" color; diff --git a/sw/ground_segment/tmtc/server_globals.ml b/sw/ground_segment/tmtc/server_globals.ml index 32cbd0052d6..6c7843a268e 100644 --- a/sw/ground_segment/tmtc/server_globals.ml +++ b/sw/ground_segment/tmtc/server_globals.ml @@ -4,7 +4,7 @@ let hostname = ref "localhost" (** FIXME: Should be read from messages.xml *) let fixedwing_ap_modes = [|"MANUAL";"AUTO1";"AUTO2";"HOME";"NOGPS";"FAIL"|] -let rotorcraft_ap_modes = [|"KILL";"SAFE";"HOME";"RATE";"ATT";"R_RCC";"A_RCC";"ATT_C";"R_ZH";"A_ZH";"HOVER";"HOV_C";"H_ZH";"NAV";"RC_D";"CF";"FWD"|] +let rotorcraft_ap_modes = [|"KILL";"SAFE";"HOME";"RATE";"ATT";"R_RCC";"A_RCC";"ATT_C";"R_ZH";"A_ZH";"HOVER";"HOV_C";"H_ZH";"NAV";"RC_D";"CF";"FWD";"MODULE"|] let _AUTO2 = 2 let gaz_modes = [|"MANUAL";"THROTTLE";"CLIMB";"ALT"|] let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|]