Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
wedgebug.c
Go to the documentation of this file.
1/*
2 * Copyright (C) Ralph Rudi schmidt <ralph.r.schmidt@outlook.com>
3
4 *
5 * This file is part of paparazzi
6 *
7 * paparazzi is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2, or (at your option)
10 * any later version.
11 *
12 * paparazzi is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with paparazzi; see the file COPYING. If not, see
19 * <http://www.gnu.org/licenses/>.
20
21 */
33/*
34 *Note. Most variables are initialized in the initialization function. Variables
35 *marked with an "!" were taken into account when calculating the total memory consumption
36 */
37
38
39// New section: Importing headers ----------------------------------------------------------------------------------------------------------------
40
41#include <stdio.h>
44#include "modules/computer_vision/cv.h" // Required for the "cv_add_to_device" function
45#include "modules/computer_vision/lib/vision/image.h"// For image-related structures
46#include "pthread.h"
47#include <stdint.h> // Needed for common types like uint8_t
48#include "state.h"
49#include "math/pprz_algebra_float.h"// Needed for vector operations, Euclidean distance, FloatVect3 and FloatRMat
50#include "math/pprz_algebra.h"// Needed for vector operations (simple subtraction)
51#include "math/pprz_geodetic_float.h"// Needed for NedCoor_f
52#include "generated/flight_plan.h" // Needed for WP (waypoint) functions and WP IDs (as defined in "ralphthesis2020_stereo_cyberzoo.xml")
53#include "firmwares/rotorcraft/autopilot_guided.h" // Needed for guidance functions such as "autopilot_guided_goto_ned" and "guidance_h_set_heading"
54#include <math.h> // needed for basic math functions
55#include "autopilot.h" // Needed to set states (GUIDED vs NAV)
56#include <time.h> // Needed to measure time
57
58
59// New section --------------------------------------------------------------------------------------------------------------------------------------
60// Defines
61#ifndef WEDGEBUG_CAMERA_RIGHT_FPS
62#define WEDGEBUG_CAMERA_RIGHT_FPS 0 //< Default FPS (zero means run at camera fps)
63#endif
64#ifndef WEDGEBUG_CAMERA_LEFT_FPS
65#define WEDGEBUG_CAMERA_LEFT_FPS 0 //< Default FPS (zero means run at camera fps)
66#endif
67
68
69
70// New section ----------------------------------------------------------------------------------------------------------------
71// Define global variables
72
73// Declaring images
77struct image_t img_left_int8_cropped; // Image obtained from left camera, converted into 8bit gray image
81struct image_t img_middle_int8_cropped; // Image obtained after processing (morphological operations) of previous image
83
86
87// New section: Global variables - Declarations ----------------------------------------------------------------------------------------------------------------
88
89
90
91// Declaring crop_t structure for information about the cropped image (after BM)
93
94// Declaring dimensions of images and kernels used
96struct img_size_t
98struct img_size_t
100
101// Declaring empty kernel for obtaining median
104
105// Delcaring structuring element sizes
114
115
116// Declaring vectors to hold global 3d points
117struct FloatVect3
119struct FloatVect3
125struct FloatVect3
127struct FloatVect3
129struct FloatVect3
131struct FloatVect3
133struct FloatVect3
135struct FloatVect3
137struct FloatVect3
139
140
141// Declaring thresholds
148
151
152// Declaring confidence parameters
157
158//int16_t edge_found_micro_confidence; //! This is the confidence that an edge was found - inside of the find_best_edge_coordinates function
168
169// Declaring boolean flags
170uint8_t is_start_reached_flag; // Set to 1 if start position is reached, 0 otherwise.
180uint8_t save_images_flag; // For report: Flag to indicate if images should be saved
181
182// Declaring principal points
185
186// Declaring edge search area
188
189
190// Declaring rotation matrices and transition vectors for frame to frame transformations
191// 1) Rotation matrix and transition vector to transform from world ENU frame to world NED frame
194// 2) Rotation matrix and transition vector to transform from world ned frame to robot frame
197// 3) Rotation matrix and transition vector to transform from robot frame to camera frame
200
201// Declaration and initialization of camera parameters
203 1000.00;
205
206
207
208// Define new structures + enums
217enum navigation_state current_state ;// Default state is 0 i.e. nothing
218
219
225
226enum control_mode_state current_mode;// Default state is 0 i.e. nothing
227
228
229
230// This is a structure to save angles that are needed for the "Edge Scan" state
231struct ES_angles {
232 float heading_initial; // This is the initial heading of the robot when the "Edge Scan" state is entered
233 float heading_max_left; // This is the maximum left turn angle (determines how much to the left the robot can look to search for edges), to search for edges
234 float heading_max_right;// This is the maximum right turn angle (determines how much to the right the robot can look to search for edges), to search for edges
235 uint8_t initiated; // This is a flag that can be set to check whether the structure is allowed to be overwritten (0=allowed, 1=forbidden)
236 uint8_t is_left_reached_flag; // This is a flag to check whether the left was scanned for an edge already
237 uint8_t is_right_reached_flag; // This is a flag to check whether the right was scanned for an edge already
238};
240
241
242// Declaring variables for time measurement
243double time_state[NUMBER_OF_STATES]; // Double array for saving total time (clock cycles) spent in the states (position 0 = state 0 and so on)
244double counter_state[NUMBER_OF_STATES]; // A counter to measure the total cycles that each state in the FSM (within the periodic function) went through
245double counter_cycles; // A counter to measure the total cycles that the periodic function went through
246clock_t clock_total_time; // Clock to measure total time (clock cycles)) it took for the robot to fly from start to goal
247clock_t clock_total_time_current; // Clock to hold time measured at start of the current cycle of the periodic function
248clock_t clock_total_time_previous; // Clock to hold time measured at start of the previous cycle of the periodic function
251
252// Other declarations
258float heading;
260 2;
263float distance_traveled; // Variable to hold the distance traveled of the robot (since start and up to the goal)
264uint8_t number_of_states; // Variable to save the total number of states used in the finite state machine
267
268
269// For debugging purpose. Allows for changing of state in simulation if 1. If 0, does not allow for state change. Useful if you want to execute only one state repeatedly
270
271uint8_t allow_state_change_MOVE_TO_GOAL; // From within state "MOVE_TO_GOAL"
272uint8_t allow_state_change_POSITION_GOAL; // From within state "POSITION_GOAL"
273uint8_t allow_state_change_WEDGEBUG_START; // From within state "WEDGEBUG_START"
274uint8_t allow_state_change_MOVE_TO_EDGE; // From within state "MOVE_TO_EDGE"
275uint8_t allow_state_change_POSITION_EDGE; // From within state "POSITION_EDGE"
276uint8_t allow_state_change_EDGE_SCAN; // From within state "EDGE_SCAN"
277
278
281
282
283// New section: Functions - Declaration ----------------------------------------------------------------------------------------------------------------
284
285// Supporting
286const char *get_img_type(enum image_type img_type); // Function 1: Displays image type
287void show_image_data(struct image_t *img); // Function 2: Displays image data
289 const char *img_name); // Function 3: Displays pixel value of image
290void show_rotation_matrix(struct FloatRMat *R);
291
292//Core
293static struct image_t *copy_left_img_func(struct image_t
294 *img, uint8_t camera_id); // Function 1: Copies left image into a buffer (buf_left)
295static struct image_t *copy_right_img_func(struct image_t
296 *img, uint8_t camera_id); // Function 2: Copies left image into a buffer (buf_right)
297void UYVYs_interlacing_V(struct image_t *YY, struct image_t *left,
298 struct image_t *right); // Function 3: Copies gray pixel values of left and right UYVY images into merged YY image
299void UYVYs_interlacing_H(struct image_t *merged, struct image_t *left, struct image_t *right);
300
302void thresholding_img(struct image_t *img, uint8_t threshold);
303void principal_points(struct point_t *c_output, const struct point_t *c_old_input, struct crop_t *img_cropped_info);
304float disp_to_depth(const uint8_t d, const float b, const uint16_t f);
305uint8_t depth_to_disp(const float depth, const float b, const uint16_t f);
307 const float b, const uint16_t f);
308int32_t indx1d_a(const int32_t y, const int32_t x, const struct image_t *img);
309int32_t indx1d_b(const int32_t y, const int32_t x, const struct img_size_t *img_dims);
310int32_t indx1d_c(const int32_t y, const int32_t x, const uint16_t img_height, const uint16_t img_width);
311
312void Va_to_Vb(struct FloatVect3 *Vb, struct FloatVect3 *Va, struct FloatRMat *Rba, struct FloatVect3 *VOa);
313void Vb_to_Va(struct FloatVect3 *Va, struct FloatVect3 *Vb, struct FloatRMat *Rba, struct FloatVect3 *VOa);
314void Vw_to_Vc(struct FloatVect3 *Vc, struct FloatVect3 *Vw, struct FloatRMat *Rrw, struct FloatVect3 *VRw,
315 struct FloatRMat *Rcr, struct FloatVect3 *VCr, const uint8_t verbose);
316void Vc_to_Vw(struct FloatVect3 *Vw, struct FloatVect3 *Vc, struct FloatRMat *Rrw, struct FloatVect3 *VRw,
317 struct FloatRMat *Rcr, struct FloatVect3 *VCr, const uint8_t verbose);
318
323
325 struct image_t *img_edges, struct image_t *img_disparity, struct crop_t *edge_search_area, uint8_t threshold,
327uint8_t is_setpoint_reached(struct FloatVect3 *VGOAL, struct FloatVect3 *VCURRENTPOSITION, float threshold);
328
330uint8_t is_heading_reached(float target_angle, float current_angle, float threshold);
335
337float dispfixed_to_disp(const int16_t d);
338float disp_to_depth_16bit(const int16_t d, const float b, const uint16_t f);
340 const float depth /*depth in m*/, const uint16_t f);
342 const float b, const uint16_t f);
345 struct image_t *img_edges, struct image_t *img_depth, struct crop_t *edge_search_area, uint16_t threshold,
348
349
350
351
352// New section: Functions - Definition ----------------------------------------------------------------------------------------------------------------
353
354// Supporting:
355
356// Function 1
358{
359 switch (img_type) {
360 case IMAGE_YUV422: return "IMAGE_YUV422";
361 case IMAGE_GRAYSCALE: return "IMAGE_GRAYSCALE";
362 case IMAGE_JPEG: return "IMAGE_JPEG";
363 case IMAGE_GRADIENT: return "IMAGE_GRADIENT";
364 default: return "Image type not found";
365 }
366}
367
368
369// Function 2
371{
372 printf("Image-Type: %s\n", get_img_type(img->type));
373 printf("Image-Width: %d\n", img->w);
374 printf("Image-Height: %d\n", img->h);
375 printf("Image-Buf_Size: %d\n", img->buf_size);
376 printf("Image-Buf_Memory_Occupied: %lu\n", sizeof(img->buf));
377}
378
379
380// Function 3
381void show_image_entry(struct image_t *img, int entry_position, const char *img_name)
382{
383 printf("Pixel %d value - %s: %d\n", entry_position, img_name, ((uint8_t *)img->buf)[entry_position]);
384}
385
386// Function 4
388{
389 printf("[[%f, %f, %f]\n", R->m[0], R->m[1], R->m[2]);
390 printf(" [%f, %f, %f]\n", R->m[3], R->m[4], R->m[5]);
391 printf(" [%f, %f, %f]]\n", R->m[6], R->m[7], R->m[8]);
392
393}
394
395
396
397// External:
398
399// Function 1 - Returns the upper left coordinates of a square (x and y coordinates) and the offset in terms of width and height,
400// given the number of disparity levels and the block size used by the block matching algorithm. This is need to crop an image
417
418
419// Function 2 - Sets finite state machine state (useful for the flight path blocks)
424
425
426// Function 3 - Creates empty 8bit kernel
428{
429
430 // Set the variables
431 kernel->type = type;
432 kernel->h = height;
433 kernel->w = width;
434
435
436 // Depending on the type the size differs
437 if (type == IMAGE_YUV422) {
438 kernel->buf_size = sizeof(uint8_t) * 2 * width * height;
439 } else if (type == IMAGE_JPEG) {
440 kernel->buf_size = sizeof(uint8_t) * 2 * width * height; // At maximum quality this is enough
441 } else if (type == IMAGE_GRADIENT) {
442 kernel->buf_size = sizeof(int16_t) * width * height;
443 } else if (type == IMAGE_INT16) {
444 kernel->buf_size = sizeof(int16_t) * width * height;
445 } else {
446 kernel->buf_size = sizeof(uint8_t) * width * height;
447 }
448
449 kernel->buf_weights = malloc(kernel->buf_size);
450 kernel->buf_values = malloc(kernel->buf_size);
451}
452
453
454// Function 4 - Deletes 8bit kernel
456{
457 if (kernel->buf_weights != NULL) {
458 free(kernel->buf_weights);
459 kernel->buf_weights = NULL;
460 }
461 if (kernel->buf_values != NULL) {
462 free(kernel->buf_values);
463 kernel->buf_values = NULL;
464 }
465}
466
467
468// Function 5 - Calculates median of a 8bit image
470{
471 // Allocate an array of the same size and sort it.
472 uint32_t i, j;
473
474 uint8_t dpSorted[n];
475 for (i = 0; i < n; ++i) {
476 dpSorted[i] = a[i];
477 }
478 for (i = n - 1; i > 0; --i) {
479 for (j = 0; j < i; ++j) {
480 if (dpSorted[j] > dpSorted[j + 1]) {
482 dpSorted[j] = dpSorted[j + 1];
483 dpSorted[j + 1] = dTemp;
484 }
485 }
486 }
487
488 // Middle or average of middle values in the sorted array.
489 uint8_t dMedian = 0;
490 if ((n % 2) == 0) {
491 dMedian = (dpSorted[n / 2] + dpSorted[(n / 2) - 1]) / 2.0;
492 } else {
493 dMedian = dpSorted[n / 2];
494 }
495 return dMedian;
496}
497
498
499// Function 6 - Calculates median of a 16bit image
501{
502 // Allocate an array of the same size and sort it.
503 uint32_t i, j;
504
506 for (i = 0; i < n; ++i) {
507 dpSorted[i] = a[i];
508 }
509 for (i = n - 1; i > 0; --i) {
510 for (j = 0; j < i; ++j) {
511 if (dpSorted[j] > dpSorted[j + 1]) {
513 dpSorted[j] = dpSorted[j + 1];
514 dpSorted[j + 1] = dTemp;
515 }
516 }
517 }
518
519 // Middle or average of middle values in the sorted array.
520 uint16_t dMedian = 0;
521 if ((n % 2) == 0) {
522 dMedian = (dpSorted[n / 2] + dpSorted[(n / 2) - 1]) / 2.0;
523 } else {
524 dMedian = dpSorted[n / 2];
525 }
526 return dMedian;
527}
528
529
530
531
532// Core:
533
534// Function 1
535static struct image_t *copy_left_img_func(struct image_t *img, uint8_t camera_id __attribute__((unused)))
536{
538 //show_image_data(img);
539 //show_image_entry(&img_left, 10, "img_left");
540 return img;
541}
542
543
544// Function 2
545static struct image_t *copy_right_img_func(struct image_t *img, uint8_t camera_id __attribute__((unused)))
546{
548 //show_image_data(img);
549 //show_image_entry(&img_right, 10, "img_right");
550 return img;
551}
552
553
554// Function 3
556{
557 // Error messages
558 if (left->w != right->w || left->h != right->h) {
559 printf("The dimensions of the left and right image to not match!");
560 return;
561 }
562 if ((merged->w * merged->h) != (2 * right->w) * right->w) {
563 printf("The dimensions of the empty image template for merger are not sufficient to merge gray left and right pixel values.");
564 return;
565 }
566
567 uint8_t *UYVY_left = left->buf;
568 uint8_t *UYVY_right = right->buf;
569 uint8_t *YY = merged->buf;
570 uint32_t loop_length = left->w * right->h;
571
572 // Incrementing pointers to get to first gray value of the UYVY images
573 UYVY_left++;
574 UYVY_right++;
575
576
577 for (uint32_t i = 0; i < loop_length; i++) {
578 *YY = *UYVY_left; // Copies left gray pixel (Y) to the merged image YY, in first position
579 YY++; // Moving to second position of merged image YY
580 *YY = *UYVY_right; // Copies right gray pixel (Y) to the merged image YY, in second position
581 YY++; // Moving to the next position, in preparation to copy left gray pixel (Y) to the merged image YY
582 UYVY_left += 2; // Moving pointer to next gray pixel (Y), in the left image
583 UYVY_right += 2; // Moving pointer to next gray pixel (Y), in the right image
584 /*
585 * Note: Since the loop lenth is based on the UYVY image the size of the data should be (2 x w) x h.
586 * This is also the same size as for the new merged YY image.
587 * Thus incrementing the pointer for UYVY_left and right, in each iteration, does not lead to problems (same for YY image)
588 */
589 }
590}
591
592
593// Function 4
595{
596 // Error messages
597 if (left->w != right->w || left->h != right->h) {
598 printf("The dimensions of the left and right image to not match!");
599 return;
600 }
601 if ((merged->w * merged->h) != (2 * right->w) * right->w) {
602 printf("The dimensions of the empty image template for merger are not sufficient to merge gray left and right pixel values.");
603 return;
604 }
605
606 uint8_t *UYVY_left = left->buf;
607 uint8_t *UYVY_right = right->buf;
608 uint8_t *YY1 = merged->buf; // points to first row for pixels of left image
609 uint8_t *YY2 = YY1 + merged->w; // points to second row for pixels of right image
610
611 // Incrementing pointers to get to first gray value of the UYVY images
612 UYVY_left++;
613 UYVY_right++;
614
615 for (uint32_t i = 0; i < left->h; i++) {
616 //printf("Loop 1: %d\n", i);
617 for (uint32_t j = 0; j < left->w; j++) {
618 //printf("Loop 1: %d\n", j);
619 *YY1 = *UYVY_left;
620 *YY2 = *UYVY_right;
621 YY1++;
622 YY2++;
623 UYVY_left += 2;
624 UYVY_right += 2;
625 }
626 YY1 += merged->w; // Jumping pointer to second next row (i.e. over row with pixels from right image)
627 YY2 += merged->w; // Jumping pointer to second next row (i.e. over row with pixels from left image)
628 }
629}
630
631
632// Function 5 - Returns the maximum value in a uint8_t image
634{
635 if (img->type == IMAGE_GRAYSCALE) {
636 uint32_t max = 0;
637 for (uint32_t i = 0; i < img->buf_size; i++) {
638 uint8_t *intensity = &((uint8_t *)img->buf)[i];
639
640 if (*intensity > max) {
641 max = *intensity;
642 }
643 }
644 return max;
645 } else if (img->type == IMAGE_INT16) {
646 uint32_t max = 0;
647 for (uint32_t i = 0; i < (img->w * img->h); i++) {
648 uint16_t *intensity = &((uint16_t *)img->buf)[i];
649
650 if (*intensity > max) {
651 max = *intensity;
652 printf("%d\n", *intensity);
653 printf("location = %d\n", i);
654 }
655 }
656 return max;
657 } else {
658 printf("ERROR: function does not support image type %d. Breaking out of function.", img->type);
659 return 1;
660 }
661
662
663}
664
665
666// Function 6 - Thresholds 8bit images given and turns all values >= threshold to 255
667void thresholding_img(struct image_t *img, uint8_t threshold)
668{
669 for (uint32_t i = 0; i < img->buf_size; i++) {
670 uint8_t *intensity = &((uint8_t *)img->buf)[i];
671
672 if (*intensity >= threshold) {
673 *intensity = 127;
674 } else {
675 *intensity = 0;
676 }
677 }
678}
679
680
681// Function 7 - Calculates principal point coordinates for a cropped image, based on the x
682// and y coordinates of the cropped area (upper left-hand side: crop_y and crop_x).
688
689
690// Function 8a - Converts disparity to depth using focal length (in pixels) and baseline distance (in meters)
691float disp_to_depth(const uint8_t _d, const float _b, const uint16_t _f)
692{
693 return _b * _f / _d;
694}
695
696
697// Function 8c - Converts 16bit fixed number disparity (pixels * 16) to 16bit disparity (pixels)
699{
700 return (_d / 16.00);
701}
702
703
704// Function 8d - Converts 16bit fixed number disparity (pixels * 16) to 16bit depth (meters) using focal length (in pixels) and baseline distance (in meters)
705float disp_to_depth_16bit(const int16_t _d, const float _b, const uint16_t _f)
706{
707 return _b * _f / dispfixed_to_disp(_d);
708}
709
710
711// Function 8b - Converts depth to disparity using focal length (in pixels) and baseline distance (in meters)
712uint8_t depth_to_disp(const float _depth, const float _b, const uint16_t _f)
713{
714 return round(_b * _f / _depth);
715}
716
717
718
719
720// Function 9a - Calculates 3d points in a scene based on the 2d coordinates of the point in the
721// image plane and the depth. d in in pixels, b is in meters and f is in pixels
723 const float _b, const uint16_t _f)
724{
725 // Calculating Z
726 // In case disparity is 0 Z will be very very small to avoid detection of algorithm that
727 // calculates closest edge point to target goal
728 //printf("y=%d\n", image_point_y);
729 //printf("x=%d\n", image_point_x);
730 //printf("d=%d\n", d);
731
732
733 if (_d == 0) {
734 scene_point->z = 0.0001;
735 } else {
737 }
738
739
740 //printf("Z=%f\n", scene_point->Z);
741
742 // Calculating Y
744
745 // Calculating X
747 //printf("Y (y=%d) = %f\n", image_point->y, scene_point->Y);
748 //printf("X (x=%d) = %f\n", image_point->x, scene_point->X);
749 //printf("Z (d=%d) = %f\n", d, scene_point->Z);
750}
751
752
753// Function 9b - Calculates 3d points in a scene based on the 2d coordinates of the point in the
754// image plane and the depth. d in in pixels, b is in meters and f is in pixels
756 const float depth /*depth in m*/, const uint16_t _f)
757{
758 // Calculating Z
759 scene_point->z = depth;
760
761 // Calculating Y
763
764 // Calculating X
766 //printf("Y (y=%d) = %f\n", image_point->y, scene_point->Y);
767 //printf("X (x=%d) = %f\n", image_point->x, scene_point->X);
768 //printf("Z (d=%d) = %f\n", d, scene_point->Z);
769}
770
771
772// Function 9c - Calculates 3d points in a scene based on the 2d coordinates of the point in the
773// image plane and the depth. d in in pixels, b is in meters and f is in pixels
775 const float _b, const uint16_t _f)
776{
777 // Calculating Z
778 // In case disparity is 0 Z will be very very small to avoid detection of algorithm that
779 // calculates closest edge point to target goal
780 //printf("y=%d\n", image_point_y);
781 //printf("x=%d\n", image_point_x);
782 //printf("d=%d\n", d);
783
784
785 if (_d == 0) {
786 scene_point->z = 0.0001;
787 } else {
789 }
790
791
792 //printf("Z=%f\n", scene_point->Z);
793
794 // Calculating Y
796
797 // Calculating X
799 //printf("Y (y=%d) = %f\n", image_point->y, scene_point->Y);
800 //printf("X (x=%d) = %f\n", image_point->x, scene_point->X);
801 //printf("Z (d=%d) = %f\n", d, scene_point->Z);
802
803}
804
805
806// Function 10a - Converts 2d coordinates into 1d coordinates (for 1d arrays) - using struct image_t for dimensions
807int32_t indx1d_a(const int32_t y, const int32_t x, const struct image_t *img)
808{
809 if (x >= (img->w) || x < 0) {
810 printf("Error: index x=%d is out of bounds for axis 0 with size %d. Returning -1\n", x, img->w);
811 return -1;
812 } else if (y >= (img->h) || y < 0) {
813 printf("Error: index y=%d is out of bounds for axis 0 with size %d. Returning -1\n", y, img->h);
814 return -1;
815 } else {
816 return x + img->w * y;
817 }
818}
819
820
821// Function 10b - Converts 2d coordinates into 1d coordinates (for 1d arrays) - using struct img_size_t for dimensions
822int32_t indx1d_b(const int32_t y, const int32_t x, const struct img_size_t *_img_dims)
823{
824 if (x >= (_img_dims->w) || x < 0) {
825 printf("Error: index %d is out of bounds for axis 0 with size %d. Returning -1\n", x, _img_dims->w);
826 return -1;
827 } else if (y >= (_img_dims->h) || y < 0) {
828 printf("Error: index %d is out of bounds for axis 0 with size %d. Returning -1\n", y, _img_dims->h);
829 return -1;
830 } else {
831 return x + _img_dims->w * y;
832 }
833}
834
835// Function 10c - Converts 2d coordinates into 1d coordinates (for 1d arrays) - using two uint16_t values for dimensions
837{
838 if (x >= (img_width) || x < 0) {
839 printf("Error: index x=%d is out of bounds for axis 0 with size %d. Returning -1\n", x, img_width);
840 return -1;
841 } else if (y >= (img_height) || y < 0) {
842 printf("Error: index y=%d is out of bounds for axis 1 with size %d. Returning -1\n", y, img_height);
843 return -1;
844 } else {
845 return x + img_width * y;
846 }
847}
848
849
850// Function 11 - Function to convert point in coordinate system a to a point in the coordinate system b
851void Va_to_Vb(struct FloatVect3 *Vb, struct FloatVect3 *Va, struct FloatRMat *Rba, struct FloatVect3 *VOa)
852{
853
854 struct FloatVect3 Va_temp;
855 Va_temp.x = Va->x;
856 Va_temp.y = Va->y;
857 Va_temp.z = Va->z;
858
859
860 // The following translates world vector coordinates into the agent coordinate system
861 Va_temp.x = Va->x - VOa->x;
862 Va_temp.y = Va->y - VOa->y;
863 Va_temp.z = Va->z - VOa->z;
864
865 // In case the axes of the world coordinate system (w) and the agent coordinate system (a) do not
866 // coincide, they are adjusted with the rotation matrix R
868}
869
870
871// Function 12 - Function to convert point in coordinate system b back to a point in the coordinate system a
872void Vb_to_Va(struct FloatVect3 *Va, struct FloatVect3 *Vb, struct FloatRMat *Rba, struct FloatVect3 *VOa)
873{
874 // In case the axes of the agent coordinate system (a) and the world coordinate system (w) do not
875 // coincide, they are adjusted with the inverse rotation matrix R
877
878 // The following translates agent vector coordinates into the world coordinate system
879 Va->x = Va->x + VOa->x;
880 Va->y = Va->y + VOa->y;
881 Va->z = Va->z + VOa->z;
882}
883
884
885// Function 13 - Function wrapper to convert a point in the world coordinate system to a point in the camera coordinate system
886void Vw_to_Vc(struct FloatVect3 *Vc, struct FloatVect3 *Vw, struct FloatRMat *Rrw, struct FloatVect3 *VRw,
887 struct FloatRMat *_Rcr, struct FloatVect3 *_VCr, const uint8_t verbose)
888{
889 struct FloatVect3 Vr;
890
891 // Print log only if enabled
892 if (verbose != 0) {
893 // Rotation matrix - World coordinate system expressed in the robot coordinate system
894 printf("Rrw\n");
895 printf("%f, %f, %f,\n", Rrw->m[0], Rrw->m[1], Rrw->m[2]);
896 printf("%f, %f, %f,\n", Rrw->m[3], Rrw->m[4], Rrw->m[5]);
897 printf("%f, %f, %f\n\n", Rrw->m[6], Rrw->m[7], Rrw->m[8]);
898
899 // Vector coordinates - Robot in the world frame system
900 printf("VRw (drone location)\n");
901 printf(" %f\n %f\n %f\n\n", VRw->x, VRw->y, VRw->z);
902
903 // World coordinates
904 printf("Vw\n");
905 printf(" %f\n %f\n %f\n\n", Vw->x, Vw->y, Vw->z);
906 }
907
908
909 // Robot coordinates from world coordinates
910 Va_to_Vb(&Vr, Vw, Rrw, VRw);
911
912 // Print log only if enabled
913 if (verbose != 0) {
914 // Robot coordinates
915 printf("Vr\n");
916 printf(" %f\n %f\n %f\n\n", Vr.x, Vr.y, Vr.z);
917
918 // Rotation matrix - Robot coordinate system expressed in the camera coordinate system
919 printf("Rcr\n");
920 printf("%f, %f, %f,\n", _Rcr->m[0], _Rcr->m[1], _Rcr->m[2]);
921 printf("%f, %f, %f,\n", _Rcr->m[3], _Rcr->m[4], _Rcr->m[5]);
922 printf("%f, %f, %f\n\n", _Rcr->m[6], _Rcr->m[7], _Rcr->m[8]);
923
924 // Vector coordinates - Camera in the robot frame system
925 printf("VCa (camera location)\n");
926 printf(" %f\n %f\n %f\n\n", _VCr->x, _VCr->y, _VCr->z);
927 }
928
929 // Camera coordinates from robot coordinates
930 Va_to_Vb(Vc, &Vr, _Rcr, _VCr);
931
932 // Print log only if enabled
933 if (verbose != 0) {
934 // Camera coordinates
935 printf("Vc\n");
936 printf(" %f\n %f\n %f\n\n", Vc->x, Vc->y, Vc->z);
937 }
938}
939
940
941// Function 14 - Function wrapper to convert a point in the camera coordinate system back to a point in the world coordinate system
942void Vc_to_Vw(struct FloatVect3 *Vw, struct FloatVect3 *Vc, struct FloatRMat *Rrw, struct FloatVect3 *VRw,
943 struct FloatRMat *_Rcr, struct FloatVect3 *_VCr, const uint8_t verbose)
944{
945 struct FloatVect3 Vr;
946
947 // Agent coordinates from camera coordinates
948 Vb_to_Va(&Vr, Vc, _Rcr, _VCr);
949
950 // Print log only if enabled
951 if (verbose != 0) {
952 // Back to robot coordinates
953 printf("Vr - back calculated\n"); \
954 printf(" %f\n %f\n %f\n\n", Vr.x, Vr.y, Vr.z);
955 }
956
957
958 // World coordinates from a coordinates
959 Vb_to_Va(Vw, &Vr, Rrw, VRw);
960
961 // Print log only if enabled
962 if (verbose != 0) {
963 // Back to world coordinates
964 printf("Vw - back calculated\n");
965 printf(" %f\n %f\n %f\n\n", Vw->x, Vw->y, Vw->z);
966 }
967}
968
969// Function 15 - Function to obtain the Euclidean distance (in meters) between two 3d points
976
977// Function 16a - function to calculate angle between robot and specific waypoint
979{
980 struct FloatVect2 VWPwt = {WaypointX(wp), WaypointY(wp)};
981 struct FloatVect2 difference;
982 float angle;
983
985 angle = atan2f(difference.x, difference.y);
986 return angle;
987}
988
989
990// Function 16b - function to calculate angle between robot and specific setpoint
992{
993 struct FloatVect2 difference;
994 float angle;
995
997 angle = atan2f(difference.y, difference.x);
998 return angle;
999}
1000
1001
1002// Function 17a - Function to calculate median disparity to a point (Vi) in an image (img), using a kernel structure (kernel_median)
1004{
1005 // Creating Start and stop coordinates of in the image coordinate system, based on kernel size
1006 uint8_t VSTARTi_y = Vi->y - (kernel_median->h / 2);
1007 uint8_t VSTARTi_x = Vi->x - (kernel_median->w / 2);
1008 uint8_t VSTOPi_y = Vi->y + (kernel_median->h / 2);
1009 uint8_t VSTOPi_x = Vi->x + (kernel_median->w / 2);
1010
1011 // In case the upper bounds of the kernel are outside of the image area
1012 // (lower bound not important because of uint8_t type converting everything below 0 to 0):
1013 if (VSTOPi_y > img->h) {
1014 VSTOPi_y = (img->h - 1);
1015 }
1016 if (VSTOPi_x > img->w) {
1017 VSTOPi_x = (img->w - 1);
1018 }
1019
1020
1021
1022 // Declaring kernel coordinates
1023 uint16_t Vk_y;
1024 uint16_t Vk_x;
1025
1026 // Declaring 1d indices (for result of transforming 2d coordinates into 1d coordinate)
1029
1030 // Declaring variable to store median in
1032
1033
1034 // Here we get the median value of a block in the middle of an image using a kernel structure
1035 for (uint8_t Vi_y = VSTARTi_y; Vi_y < (VSTOPi_y + 1); Vi_y++) {
1036 for (uint8_t Vi_x = VSTARTi_x; Vi_x < (VSTOPi_x + 1); Vi_x++) {
1037 // Calculating kernel coordinates
1038 Vk_y = Vi_y - VSTARTi_y;
1039 Vk_x = Vi_x - VSTARTi_x;
1040
1041 // Converting 2d indices to 1d indices
1044
1045
1046 // Saving disparity values of image underneath the kernel, into the kernel buffer
1048 }
1049 }
1050
1051 // Calculating median disparity value of values recoded by the kernel
1053
1054 return median;
1055}
1056
1057
1058// Function 17b - Function to calculate median depth (cm) to a point (Vi) in a 16bit image (img), using a kernel structure (kernel_median)
1060{
1061 // Creating Start and stop coordinates of in the image coordinate system, based on kernel size
1062 uint16_t VSTARTi_y = Vi->y - (kernel_median->h / 2);
1063 uint16_t VSTARTi_x = Vi->x - (kernel_median->w / 2);
1064 uint16_t VSTOPi_y = Vi->y + (kernel_median->h / 2);
1065 uint16_t VSTOPi_x = Vi->x + (kernel_median->w / 2);
1066
1067 // In case the upper bounds of the kernel are outside of the image area
1068 // (lower bound not important because of uint8_t type converting everything below 0 to 0):
1069 if (VSTOPi_y > img->h) {
1070 VSTOPi_y = (img->h - 1);
1071 }
1072 if (VSTOPi_x > img->w) {
1073 VSTOPi_x = (img->w - 1);
1074 }
1075
1076
1077
1078 // Declaring kernel coordinates
1079 uint16_t Vk_y;
1080 uint16_t Vk_x;
1081
1082 // Declaring 1d indices (for result of transforming 2d coordinates into 1d coordinate)
1085
1086 // Declaring variable to store median in
1088
1089
1090 // Here we get the median value of a block in the middle of an image using a kernel structure
1091 for (uint16_t Vi_y = VSTARTi_y; Vi_y < (VSTOPi_y + 1); Vi_y++) {
1092 for (uint16_t Vi_x = VSTARTi_x; Vi_x < (VSTOPi_x + 1); Vi_x++) {
1093 // Calculating kernel coordinates
1094 Vk_y = Vi_y - VSTARTi_y;
1095 Vk_x = Vi_x - VSTARTi_x;
1096
1097 // Converting 2d indices to 1d indices
1100
1101
1102 // Saving disparity values of image underneath the kernel, into the kernel buffer
1104
1105 }
1106 }
1107
1108 // Calculating median disparity value of values recoded by the kernel
1110
1111 return median;
1112}
1113
1114
1115
1116
1117// Function 18a - Function to find "best" (closest ideal pathway to goal from robot to edge to goal) - Using disparity image
1118// Returns a 3d Vector to the best "edge" and 1 if any edge is found and 0 if no edge is found.
1120 struct FloatVect3
1121 *_VEDGECOORDINATESc, // Declared vector of coordinates of "best" edge detected in camera coordinate system
1122 struct FloatVect3 *VTARGETc, // Declared vector of coordinates of goal in camera coordinate system
1123 struct image_t *img_edges, // Image obtained from the external Sobel edge detection function = sobel_OCV
1124 struct image_t *img_disparity, // Image obtained after simple block matching
1125 struct crop_t *_edge_search_area, // This structure holds information about the window in which edges are searched in
1126 uint8_t threshold, // Above this disparity (pixels) edges are eligible for the WedgeBug algorithm (i.e. edges cannot be very far away)
1127 int16_t max_confidence // This is the max confidence that edges were found - Works like a threshold
1128)
1129{
1130 // Declarations
1131 struct FloatVect3
1132 VEDGEc; // A vector to save the point of an eligible detected edge point in the camera coordinate system.
1133 struct FloatVect3 VROBOTCENTERc; // A vector to hold camera center coordinates in the camera coordinate system.
1134 struct point_t
1135 VCLOSESTEDGEi; // A vector to save the point of the "best" eligible detected edge point in the image coordinate system.
1136 float f_distance_edge_to_goal; // Saves distance from edge to goal
1137 float f_distance_robot_to_edge; // Saves distance from robot to goal
1138 float distance; // This stores distance from edge to goal. Its initialized with 255 as basically any edge found will be closer than that and will replace 255 met
1139 uint8_t edge_value; // Variable to store the intensity value of a pixel in the img_edge
1140 uint8_t disparity; // variable to store the disparity level of a pixel in the img_disparity
1141 uint8_t disparity_best; // Variable to store disparity level of associated best edge found (for debugging)
1142 int16_t confidence; // Confidence = number of edge pixels found
1143 int32_t indx; // Variable to store 1d index calculated from 2d index
1144
1145 // Initializations
1146 VROBOTCENTERc.x = 0.0; VROBOTCENTERc.y = 0.0; VROBOTCENTERc.z = 0.0;
1147 VCLOSESTEDGEi.x = 0; VCLOSESTEDGEi.y = 0;
1148 distance = 255;
1149 confidence = 0;
1150 disparity_best = 0;
1151
1152
1153 // Loop to calculate position (in image) of point closes to hypothetical target - Start
1155 y++) { //for (uint32_t y = _edge_search_area.y; y < (_edge_search_area.y + _edge_search_area.h); y++)
1157 indx = indx1d_a(y, x, img_edges); // We convert the 2d index [x,y] into a 1d index
1158 edge_value = ((uint8_t *) img_edges->buf)[indx]; // We save the intensity of the current point
1159 disparity = ((uint8_t *) img_disparity->buf)[indx]; // We save the disparity of the current point
1160
1161 // Two conditions must be met for an edge to be considered a viable route for the drone:
1162 // 1) This disparity of the current coordinate (x,y) must coincide with an edge pixel
1163 // (as all non-edge pixels have been set to 0) - (edge_value != 0)
1164 // 2) The disparity of the current coordinate (x, y) must be above a certain threshold. This simulates vision cone - (disparity > threshold_disparity_of_edges)
1165 if ((edge_value != 0) && (disparity > threshold)) {
1166 // We increase the confidence for every edge found
1167 confidence++;
1169
1170 // We determine the offset from the principle point
1171 int32_t y_from_c = y - c_img_cropped.y; // NOTE. The variable "c_img_cropped" is a global variable
1172 int32_t x_from_c = x - c_img_cropped.x; // NOTE. The variable "c_img_cropped" is a global variable
1173 // We derive the 3d scene point using from the disparity saved earlier
1174 Vi_to_Vc(&VEDGEc, y_from_c, x_from_c, disparity, b, f); // NOTE. The variables "b" and "f" are a global variables
1175 // Calculating Euclidean distance (N2) - Edge to goal
1177 // Calculating Euclidean distance (N2) - robot to edge
1179
1180
1181 // If current distance (using distance vector) is smaller than the previous minimum distance
1182 // measure then save new distance and point coordinates associated with it
1184 // Saving closest edge point, in camera coordinate system
1188 // Saving disparity at point
1190 // Saving smallest distance
1192 // Saving closest edge point, in image coordinate system
1193 VCLOSESTEDGEi.y = y;
1194 VCLOSESTEDGEi.x = x;
1195
1196 }
1197
1198 //printf("x image = %d\n", x);
1199 //printf("y image = %d\n", y);
1200 //printf("x image from c = %d\n", x_from_c);
1201 //printf("y image from c = %d\n", y_from_c);
1202 //printf("d = %d\n", disparity);
1203 //printf("X scene from c = %f\n", scene_point.X);
1204 //printf("Y scene from c = %f\n", scene_point.Y);
1205 //printf("Z scene from c = %f\n", scene_point.Z);
1206 //printf("Closest edge [y,x] = [%d, %d]\n", (int)closest_edge.y, (int)closest_edge.x);
1207 //printf("Robot center coordinates = [%f, %f, %f]\n", VROBOTCENTERc.x, VROBOTCENTERc.y, VROBOTCENTERc.z);
1208 //printf("Edge coordinates = [%f, %f, %f]\n", VEDGEc.x, VEDGEc.y, VEDGEc.z);
1209 //printf("Distance to goal (m) = %f\n", distance);
1210 //printf("Distance to goal2 (m) = %f + %f\n\n", f_distance_robot_to_edge, f_distance_edge_to_goal);
1211 }
1212 }
1213 }
1214
1215 if (confidence == max_confidence) {
1217 printf("Viable closest edge found: [%d, %d] (disparity = %d)\n", VCLOSESTEDGEi.y, VCLOSESTEDGEi.x, disparity_best);
1218
1219 printf("At distance: %f\n", distance);
1220 confidence = 0;
1221 return 1;
1222 } else {
1223 printf("No viable edge found\n");
1224 confidence = 0;
1225 return 0;
1226 }
1227}
1228
1229
1230
1231// Function 18b - Function to find "best" (closest ideal pathway to goal from robot to edge to goal) - Using depth image
1232// Returns a 3d Vector to the best "edge" and 1 if any edge is found and 0 if no edge is found.
1234 struct FloatVect3
1235 *_VEDGECOORDINATESc, // Declared vector of coordinates of "best" edge detected in camera coordinate system
1236 struct FloatVect3 *VTARGETc, // Declared vector of coordinates of goal in camera coordinate system
1237 struct image_t *img_edges, // Image obtained from the external Sobel edge detection function = sobel_OCV
1238 struct image_t *img_depth, // Image holding depth values (cm) obtained from the disparity image
1239 struct crop_t *_edge_search_area, // This structure holds information about the window in which edges are searched in
1240 uint16_t threshold, // Below this depth (cm) edges are eligible for the WedgeBug algorithm (i.e. edges cannot be very far away)
1241 int16_t max_confidence // This is the max confidence that edges were found - Works like a threshold
1242)
1243{
1244 // Declarations
1245 struct FloatVect3
1246 VEDGEc; // A vector to save the point of an eligible detected edge point in the camera coordinate system.
1247 struct FloatVect3 VROBOTCENTERc; // A vector to hold camera center coordinates in the camera coordinate system.
1248 struct point_t
1249 VCLOSESTEDGEi; // A vector to save the point of the "best" eligible detected edge point in the image coordinate system.
1250 float f_distance_edge_to_goal; // Saves distance from edge to goal
1251 float f_distance_robot_to_edge; // Saves distance from robot to goal
1252 float distance; // This stores distance from edge to goal. Its initialized with 255 as basically any edge found will be closer than that and will replace 255 meters
1253 uint8_t edge_value; // Variable to store the intensity value of a pixel in the img_edge
1254 uint16_t depth; // Variable to store the depth (in cm) of a pixel in the img_disparity
1255 uint16_t depth_best; // Variable to store depth (cm) level of associated best edge found (for debugging)
1256 int16_t confidence; // Confidence = number of edge pixels found
1257 int32_t indx; // Variable to store 1d index calculated from 2d index
1258
1259 // Initializations
1260 VROBOTCENTERc.x = 0.0; VROBOTCENTERc.y = 0.0; VROBOTCENTERc.z = 0.0;
1261 VCLOSESTEDGEi.x = 0; VCLOSESTEDGEi.y = 0;
1262 distance = 255;
1263 confidence = 0;
1264 depth_best = 0;
1265
1266
1267 // Loop to calculate position (in image) of point closes to hypothetical target - Start
1269 y++) { //for (uint32_t y = _edge_search_area.y; y < (_edge_search_area.y + _edge_search_area.h); y++)
1271 indx = indx1d_a(y, x, img_edges); // We convert the 2d index [x,y] into a 1d index
1272 edge_value = ((uint8_t *) img_edges->buf)[indx]; // We save the intensity of the current point
1273 depth = ((uint16_t *) img_depth->buf)[indx];
1274
1275 // Two conditions must be met for an edge to be considered a viable route for the drone:
1276 // 1) This disparity of the current coordinate (x,y) must coincide with an edge pixel
1277 // (as all non-edge pixels have been set to 0) - (edge_value != 0)
1278 // 2) The disparity of the current coordinate (x, y) must be above a certain threshold. This simulates vision cone - (disparity > threshold_disparity_of_edges)
1279 if ((edge_value != 0) && (depth < threshold)) {
1280 // We increase the confidence for every edge found
1281 confidence++;
1283
1284 // We determine the offset from the principle point
1285 int32_t y_from_c = y - c_img_cropped.y; // NOTE. The variable "c_img_cropped" is a global variable
1286 int32_t x_from_c = x - c_img_cropped.x; // NOTE. The variable "c_img_cropped" is a global variable
1287 // We derive the 3d scene point using the depth from the depth image (img_depth). Note depth is in cm, but function takes m. Thus, we convert
1289 f); // NOTE. The variables "b" and "f" are a global variables
1290
1291 // Calculating Euclidean distance (N2) - Edge to goal
1293 // Calculating Euclidean distance (N2) - robot to edge
1295
1296
1297 // If current distance (using distance vector) is smaller than the previous minimum distance
1298 // measure then save new distance and point coordinates associated with it
1300 // Saving closest edge point, in camera coordinate system
1304 // Saving disparity at point
1305 depth_best = depth;
1306 // Saving smallest distance
1308 // Saving closest edge point, in image coordinate system
1309 VCLOSESTEDGEi.y = y;
1310 VCLOSESTEDGEi.x = x;
1311
1312 }
1313
1314 //printf("x image = %d\n", x);
1315 //printf("y image = %d\n", y);
1316 //printf("x image from c = %d\n", x_from_c);
1317 //printf("y image from c = %d\n", y_from_c);
1318 //printf("d = %d\n", disparity);
1319 //printf("X scene from c = %f\n", scene_point.X);
1320 //printf("Y scene from c = %f\n", scene_point.Y);
1321 //printf("Z scene from c = %f\n", scene_point.Z);
1322 //printf("Closest edge [y,x] = [%d, %d]\n", (int)closest_edge.y, (int)closest_edge.x);
1323 //printf("Robot center coordinates = [%f, %f, %f]\n", VROBOTCENTERc.x, VROBOTCENTERc.y, VROBOTCENTERc.z);
1324 //printf("Edge coordinates = [%f, %f, %f]\n", VEDGEc.x, VEDGEc.y, VEDGEc.z);
1325 //printf("Distance to goal (m) = %f\n", distance);
1326 //printf("Distance to goal2 (m) = %f + %f\n\n", f_distance_robot_to_edge, f_distance_edge_to_goal);
1327 }
1328 }
1329 }
1330
1331 if (confidence == max_confidence) {
1333 printf("Viable closest edge found: [%d, %d] (depth = %f)\n", VCLOSESTEDGEi.y, VCLOSESTEDGEi.x, (depth_best / 100.00));
1334
1335 printf("At distance: %f\n", distance);
1336 confidence = 0;
1337 return 1;
1338 } else {
1339 printf("No viable edge found\n");
1340 confidence = 0;
1341 return 0;
1342 }
1343}
1344
1345
1346
1347// Function 19 - Function to determine if setpoint was reached (it was reached if distance is below a set threshold)
1349{
1350 if (float_vect3_norm_two_points(VGOAL, VCURRENTPOSITION) < threshold) {return 1;}
1351 else {return 0;}
1352}
1353
1354
1355// Function 20 - function to obtain the Euclidian distance between two angles (in radians)
1357{
1358 float target_current_diff;
1360 //printf("Target angle = %f\n", target_angle);
1361 //printf("Current angle = %f\n", current_angle);
1362 //printf("difference in function = %f\n", sqrt(pow(target_current_diff, 2)));
1363 return sqrt(pow(target_current_diff, 2));
1364}
1365
1366// Function 21 - Function to determine if set heading was reached (it was reached if the Euclidean distance is below a set threshold)
1368{
1369 if (float_norm_two_angles(target_angle, current_angle) < threshold) {return 1;}
1370 else {return 0;}
1371}
1372
1373// Function 22
1382
1383
1384// Function 23 - Function to convert 16bit disparity (unaltered from OCV output i.e. fixed point 16bit numbers) into 16bit depth values (cm)
1386{
1387 //int n = 89;
1388 float disparity = 1;
1389
1390 //printf("C location %d = %d\n", n, ((int16_t*)img16bit_input->buf)[n]);
1391
1392
1393 // Converting disparity values into depth (cm)
1394 for (int32_t i = 0; i < (img_input->h * img_input->w); i++) {
1395
1396 if (img_input->type == IMAGE_GRAYSCALE) {
1397 disparity = disp_to_depth(((uint8_t *)img_input->buf)[i], b, f);
1398 } else if (img_input->type == IMAGE_INT16) {
1400 } else {
1401 printf("ERROR: function does not support image type %d. Breaking out of function.", img_input->type);
1402 }
1403
1404
1405 /*
1406 if(i == n)
1407 {
1408 printf("Depth in meters at %d = %f\n", n, disparity);
1409 }*/
1410
1411 disparity = disparity * 100;
1412
1413 /*if(i == n)
1414 {
1415 printf("Depth in cm at %d = %f\n", n, disparity);
1416 }*/
1417
1418
1419 ((uint16_t *)img16bit_output->buf)[i] = round(disparity);
1420 }
1421 //printf("Depth in cm at %d = %d\n", n, ((int16_t*)img16bit_output->buf)[n]);
1422}
1423
1424// Function 24 - Function that encapsulates all of the background processes. Originally this was in the periodic function,
1425// but since it is used in the direct and the guidance navigation modes of the state machine, I made it a callable function
1426// for convenience
1428{
1429 //Background processes
1430 // 1. Converting left and right image to 8bit grayscale for further processing
1431 image_to_grayscale(&img_left, &img_left_int8); // Converting left image from UYVY to gray scale for saving function
1432 image_to_grayscale(&img_right, &img_right_int8); // Converting right image from UYVY to gray scale for saving function
1433
1434 // 2. Deriving disparity map from block matching (left image is reference image)
1436 1);// Creating cropped disparity map image
1437 // For report: creating image for saving 1
1438 if (_save_images_flag) {save_image_HM(&img_disparity_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img1_post_SBM.bmp", heat_map_type);}
1439
1440 /*
1441 // Optional thresholding of disparity map
1442 uint8_t thresh = 1;
1443 for (int32_t i = 0; i < (img_disparity_int8_cropped.h*img_disparity_int8_cropped.w); i++)
1444 {
1445 uint8_t disparity = ((uint8_t*)img_disparity_int8_cropped)[i];
1446 if(disparity < thresh)
1447 {
1448 ((uint8_t*)img_disparity_int8_cropped)[i] = 0; // if below disparity assume object is indefinately away
1449 }
1450 }*/
1451
1452 // 3. Morphological operations 1
1453 // Needed to smoove object boundaries and to remove noise removing noise
1455 // For report: creating image for saving 2
1456 if (_save_images_flag) {save_image_HM(&img_middle_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img2_post_opening_8bit.bmp", heat_map_type);}
1457
1459 // For report: creating image for saving 3
1460 if (_save_images_flag) {save_image_HM(&img_middle_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img3_post_closing_8bit.bmp", heat_map_type);}
1461
1463 // For report: creating image for saving 4
1464 if (_save_images_flag) {save_image_HM(&img_middle_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img4_post_dilation_8bit.bmp", heat_map_type);}
1465
1466 // 4. Depth image
1468 // For report: creating image for saving 4
1469 if (_save_images_flag) {save_image_HM(&img_depth_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img5_post_depth_16bit.bmp", heat_map_type);}
1470
1471 // 5. Sobel edge detection
1473 // For report: creating image for saving 5
1474 if (_save_images_flag) {save_image_gray(&img_edges_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b_img6_post_sobel_8bit.bmp");}
1475}
1476
1477
1478// Function 25 - Function that encapsulates all of the background processes. Originally this was in the periodic function,
1479// but since it is used in the direct and the guidance navigation modes of the state machine, I made it a callable function
1480// for convenience
1482{
1483 //Background processes
1484 // 1. Converting left and right image to 8bit grayscale for further processing
1485 image_to_grayscale(&img_left, &img_left_int8); // Converting left image from UYVY to gray scale for saving function
1486 image_to_grayscale(&img_right, &img_right_int8); // Converting right image from UYVY to gray scale for saving function
1487
1488 // 2. Deriving disparity map from block matching (left image is reference image)
1490 1);// Creating cropped disparity map image
1491 // For report: creating image for saving 1
1492 if (_save_images_flag) {save_image_HM(&img_disparity_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img1_post_SBM_16bit.bmp", heat_map_type);}
1493
1494 //printf("maximum_intensity = %d\n", maximum_intensity(&img_disparity_int16_cropped));
1495
1496 /*
1497 // Optional thresholding of disparity map
1498 uint8_t thresh = 1;
1499 for (int32_t i = 0; i < (img_disparity_int8_cropped.h*img_disparity_int8_cropped.w); i++)
1500 {
1501 uint8_t disparity = ((uint8_t*)img_disparity_int8_cropped)[i];
1502 if(disparity < thresh)
1503 {
1504 ((uint8_t*)img_disparity_int8_cropped)[i] = 0; // if below disparity assume object is indefinately away
1505 }
1506 }*/
1507
1508 // 3. Morphological operations 1
1509 // Needed to smoove object boundaries and to remove noise removing noise
1510
1512 // For report: creating image for saving 3
1513 if (_save_images_flag) {save_image_HM(&img_disparity_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img2_post_closing_16bit.bmp", heat_map_type);}
1514
1515
1517 // For report: creating image for saving 2
1518 if (_save_images_flag) {save_image_HM(&img_disparity_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img3_post_opening_16bit.bmp", heat_map_type);}
1519
1520
1521
1523 // For report: creating image for saving 4
1524 if (_save_images_flag) {save_image_HM(&img_disparity_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img4_post_dilation_16bit.bmp", heat_map_type);}
1525
1526
1527 // 4. Depth image
1529 // For report: creating image for saving 4
1530 if (_save_images_flag) {save_image_HM(&img_depth_int16_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img5_post_depth_16bit.bmp", heat_map_type);}
1531
1532
1533 // 5. Sobel edge detection
1535 // For report: creating image for saving 5
1536 if (_save_images_flag) {save_image_gray(&img_edges_int8_cropped, "/home/dureade/Documents/paparazzi_images/for_report/b2_img6_post_sobel_8bit.bmp");}
1537
1538 // 6. Morphological operations 2
1539 // This is needed so that when using the edges as filters (to work on depth values
1540 // only found on edges) the underlying depth values are those of the foreground
1541 // and not the background
1543}
1544
1545
1546
1547
1548
1549
1550/*
1551 * a: Coordinate system a (i.e. the coordinate frame a)
1552 * b: Coordinate system b (i.e. the coordinate frame b)
1553 * w: World coordinate system (i.e. the world coordinate frame = x is depth, y is left and right, and z is altitude))
1554 * r: Robot coordinate system (i.e. the robot coordinate frame = x is depth, y is left and right, and z is altitude))
1555 * c: Camera Coordinate system (i.e. the camera coordinate frame = x is left and right, y is altitude and z is depth)
1556 * i: Image coordinate system (i.e. the image (plane) coordinate frame)
1557 * k: Kernel coordinate system (i.e. the kernel coordinate frame)
1558 *
1559 * Va: Vector coordinates, in the coordinate system a (i.e. a point in the coordinate system a)
1560 * Vb: Vector coordinates, in the coordinate system b (i.e. a point in the coordinate system b)
1561 * Vwned: Vector coordinates, in the world coordinate system (i.e. a point in the world coordinate system) -NED
1562 * Vwenu: Vector coordinates, in the world coordinate system two (i.e. a point in the world coordinate system two) - ENU
1563 * Vr: Vector coordinates, in the robot coordinate system (i.e. a point in the world coordinate system)
1564 * Vc: Vector coordinates, in the camera coordinate system (i.e. a point in the world coordinate system)
1565 * Vi: Vector coordinates, in the image coordinate system (i.e. a point in the image [plane] coordinates system)
1566 * Vk: Vector coordinates, in the kernel coordinate system (i.e. a point in the kernel coordinates system)
1567 *
1568 * R: Rotation matrix
1569 *
1570 * Rba: Rotation matrix of coordinate system a expressed in the coordinate system b
1571 * Rrw: Rotation matrix of world coordinate system expressed in the robot coordinate system
1572 * Rcr: Rotation matrix of robot coordinate system expressed in the camera coordinate system
1573 *
1574 * VRw: Vector coordinates of robot in the world coordinate system
1575 * VCr: Vector coordinates of camera in the robot coordinate system
1576 * VOa: Vector coordinates of the object (has its own object frame) in the the coordinate system a *
1577 */
1578
1579
1580
1581
1582
1583// New section: Init and periodic functions ----------------------------------------------------------------------------------------------------------------
1585{
1586 //printf("Wedgebug init function was called\n");
1587
1588
1589 // Images creation process:
1590 // Creation of images
1591 // Creating structure to hold dimensions of normal camera images
1593
1594
1595 image_create(&img_left, img_dims.w, img_dims.h, IMAGE_YUV422); // To store left camera image
1596 image_create(&img_right, img_dims.w, img_dims.h, IMAGE_YUV422);// To store right camera image
1597 image_create(&img_left_int8, img_dims.w, img_dims.h, IMAGE_GRAYSCALE); // To store gray scale version of left image
1598 image_create(&img_right_int8, img_dims.w, img_dims.h, IMAGE_GRAYSCALE); // To store gray scale version of left image
1599
1600
1601 // Creation of images - Cropped:
1602 // Calculating cropped image details (x, y, width and height)
1604
1605 // Creating structure to hold dimensions of cropped camera images
1607
1608 printf("img_cropped_info [w, h, x, y] = [%d, %d, %d, %d]\n", img_cropped_info.w, img_cropped_info.h, img_cropped_info.x,
1610
1611 // Creating empty images - cropped - 8bit
1613 IMAGE_GRAYSCALE);// To store cropped depth - 8 bit
1615 IMAGE_GRAYSCALE);// To store cropped depth - 8 bit
1617 IMAGE_GRAYSCALE); // To store intermediate image data from processing - 8 bit
1619 IMAGE_GRAYSCALE); // To store edges image data from processing - 8 bit
1620
1621 // Creating empty images - cropped - 16bit
1623 IMAGE_INT16);// To store cropped disparity - 16 bit
1625 IMAGE_INT16);// To store cropped depth - 16 bit
1627 IMAGE_INT16);// To store cropped middle image - 16 bit
1628
1629
1630
1631
1632
1633 // Creation of kernels:
1634 // Creating structure to hold dimensions of kernels
1635 K_median_w = 43;
1636 K_median_h = 43;
1637
1639 // Creating empty kernel:
1642
1643 printf("median_kernel16bit [buf_size, h, w, type] = [%d, %d, %d, %d]\n", median_kernel16bit.buf_size,
1645
1646
1647
1648
1649 // Adding callback functions
1652
1653
1654 //Initialization of constant rotation matrices and transition vectors for frame to frame transformations
1655 // 1) Rotation matrix and transition vector to transform from world ENU frame to world NED frame
1656 Rwnedwenu.m[0] = 0; Rwnedwenu.m[1] = 1; Rwnedwenu.m[2] = 0;
1657 Rwnedwenu.m[3] = 1; Rwnedwenu.m[4] = 0; Rwnedwenu.m[5] = 0;
1658 Rwnedwenu.m[6] = 0; Rwnedwenu.m[7] = 0; Rwnedwenu.m[8] = -1;
1659 VNEDwenu.x = 0;
1660 VNEDwenu.y = 0;
1661 VNEDwenu.z = 0;
1662 // 2) Rotation matrix and transition vector to transform from robot frame to camera frame
1663 Rcr.m[0] = 0; Rcr.m[1] = 1; Rcr.m[2] = 0;
1664 Rcr.m[3] = 0; Rcr.m[4] = 0; Rcr.m[5] = 1;
1665 Rcr.m[6] = 1; Rcr.m[7] = 0; Rcr.m[8] = 0;
1666 VCr.x = 0;// i.e the camera is x meters away from robot center
1667 VCr.y = 0;
1668 VCr.z = 0;
1669
1670 // Initializing goal vector in the NED world coordinate system
1675
1676
1677 // Initializing start vector in the NED world coordinate system
1682
1683
1684 // Calculating principal points of normal image and cropped image
1685 c_img.y = img_dims.h / 2;
1686 c_img.x = img_dims.w / 2;
1688 &img_cropped_info); // Calculates principal points for cropped image, considering the original dimensions
1689
1690
1691 // Initializing structuring element sizes
1692 SE_opening_OCV = 13; // SE size for the opening operation
1693 SE_closing_OCV = 13; // SE size for the closing operation
1695 51;//25;//51;//301;// SE size for the first dilation operation (Decides where edges are detected, increase to increase drone safety zone NOTE. This functionality should be replaced with c space expansion)
1697 11; // SE size for the second dilation operation (see state 3 "WEDGEBUG_START" and state 6 "POSITION_EDGE")
1698 SE_sobel_OCV = 5; // SE size for the sobel operation, to detect edges
1700 11; // SE size for the erosion operation (see state 3 "WEDGEBUG_START" and state 6 "POSITION_EDGE", its needed to "drag" the depth of the foreground objects over the edges detected)
1701
1702 // Setting thresholds - non-calculated
1704 700;
1707 5000; // Edges with a magnitude above this value are detected. Above this value, edges are given the value 127, otherwise they are given the value zero.
1708 threshold_distance_to_goal = 0.25; // Above this threshold (m), the goal is considered reached
1710 1.0;//Above this threshold (m), the goal is considered reached in DIRECT_CONTROL mode
1711 threshold_distance_to_angle = 0.0004; // Above this threshold (radians), the angle/heading is considered reached
1712
1713 // Setting thresholds - calculated
1714 //threshold_median_disparity = depth_to_disp((threshold_median_depth / 100.00), b, f); //8// Above this median disparity, an obstacle is considered to block the way. >60 = close than 35cm
1715 //threshold_median_depth = (uint16_t) disp_to_depth(threshold_median_disparity, b, f) * 100;
1716 //threshold_disparity_of_edges = depth_to_disp((threshold_depth_of_edges / 100.00), b, f); //5// Above this underlying disparity value, edges are considers eligible for detection
1717
1718 // Initializing confidence parameters
1719 obstacle_confidence = 0; // This is the confidence that an obstacle was spotted
1720 free_path_confidence = 0; // This is the confidence that no obstacle was spotted
1721 position_confidence = 0; // This is the confidence that the desired position was reached
1722 heading_confidence = 0; // This is the confidence that the desired heading is reached
1723 //edge_found_micro_confidence = 0; // This is the confidence that an edge was found
1724 edge_found_macro_confidence = 0; // This is the confidence that an edge was found
1725 no_edge_found_confidence = 0; // This is the confidence that no edge was found
1726 max_obstacle_confidence = 10; // This is the max confidence that an obstacle was spotted
1727 max_free_path_confidence = 5; // This is the max confidence that an obstacle was not spotted
1728 max_position_confidence = 30; // This is the max confidence that a specific position was reached
1729 max_heading_confidence = 5; // This is the max confidence that a specific heading was reached
1730 max_edge_found_micro_confidence = 1;//50 // This is the max confidence that edges were found
1732 1; // This is the max confidence that edges were found (can be higher if angular change is slower or speed of periodic function is increase)
1733 max_no_edge_found_confidence = 10; // This is the max confidence that no edges were found
1734
1735 // Initializing boolean flags
1736 is_start_reached_flag = 0; // Set to 1 if start position is reached, 0 otherwise
1737 is_setpoint_reached_flag = 0; // Set to 1 if setpoint is reached, 0 otherwise
1738 is_obstacle_detected_flag = 0; // Set to 1 if obstacle is detected, 0 otherwise
1739 is_path_free_flag = 0; // Set to 1 if no obstacle is detected, 0 otherwise
1740 is_heading_reached_flag = 0; // Set to 1 if heading is reached, 0 otherwise
1741 is_edge_found_macro_flag = 0; // Set to 1 if best edge (according to macro confidence) was found, 0 otherwise
1742 is_edge_found_micro_flag = 0; // Set to 1 if best edge (according to micro confidence) was found, 0 otherwise
1743 is_no_edge_found_flag = 0; // Set to 1 if no edge was identified, 0 otherwise
1744 is_state_changed_flag = 0; // Set to 1 if state was changed, 0 otherwise
1746 initial_heading.is_left_reached_flag = 0; // The scan has not reached the left maximum angle yet
1747 initial_heading.is_right_reached_flag = 0;// The scan has not reached the right maximum angle yet
1748
1751
1752 save_images_flag = 1; // For report: Flag to indicate if images should be saved
1753
1754
1755
1756 // Initializing area over which edges are searched in
1757
1758 // p3DWedgeBug:
1759 edge_search_area.y = 0;
1761 edge_search_area.x = 0;
1763
1764// // p2DWedgeBug:
1765// edge_search_area.y = 216/2;
1766// edge_search_area.h = 1;
1767// edge_search_area.x = 0;
1768// edge_search_area.w = img_disparity_int8_cropped.w;
1769
1770
1771 // Initializing Edge scan structure
1772 initial_heading.initiated = 0; // 0 = it can be overwritten
1773
1774
1775
1776 // Initializing debugging options
1777
1778 allow_state_change_MOVE_TO_GOAL = 1; // Allows state change from within state "MOVE_TO_GOAL"
1779 allow_state_change_POSITION_GOAL = 1; // Allows state change from within state "POSITION_GOAL"
1780 allow_state_change_WEDGEBUG_START = 1; // Allows state change from within state "WEDGEBUG_START"
1781 allow_state_change_MOVE_TO_EDGE = 1; // Allows state change from within state "MOVE_TO_EDGE"
1782 allow_state_change_POSITION_EDGE = 1; // Allows state change from within state "POSITION_EDGE"
1783 allow_state_change_EDGE_SCAN = 1; // Allows state change from within state "EDGE_SCAN"
1784
1785 // Other initializations
1786 previous_state = 0; // Variable for state machine memory
1787 previous_mode = 0;
1789 VSTARTwned.x; // Initializing a vector to hold the current position, which is needed for calculating the distance traveled
1793 0; // Clock to measure total time (clock cycles)) it took for the robot to fly from start to goal
1794 clock_total_time_current = 0; // Clock to hold time measured at start of the current cycle of the periodic function
1795 clock_total_time_previous = 0; // Clock to hold time measured at start of the previous cycle of the periodic function
1796 distance_traveled = 0; // Variable to hold the distance traveled of the robot (since start and up to the goal)
1797 number_of_states = NUMBER_OF_STATES; // Variable to save the total number of states used in the finite state machine
1799 99999; // Variable to hold distance from robot to edge to goal (used in EDGE_SCAN (9) state) - initialized with unreasonably high number
1801 clock_FSM = 0;
1802 heat_map_type = 2; // Heat map used when saving image
1803
1804 /*
1805 * Heat maps:
1806 * 0 = COLORMAP_AUTUMN
1807 * 1 = COLORMAP_BONE
1808 * 2 = COLORMAP_JET
1809 * 3 = COLORMAP_WINTER
1810 * 4 = COLORMAP_RAINBOW
1811 * 5 = COLORMAP_OCEAN
1812 * 6 = COLORMAP_SUMMER
1813 * 7 = COLORMAP_SPRING
1814 * 8 = COLORMAP_COOL
1815 * 9 = COLORMAP_HSV
1816 * 10 = COLORMAP_PINK
1817 * 11 = COLORMAP_HOT
1818 */
1819
1820 /*
1821 enum navigation_state {
1822 MOVE_TO_GOAL = 1,
1823 POSITION_GOAL = 2,
1824 WEDGEBUG_START = 3,
1825 MOVE_TO_EDGE = 4,
1826 POSITION_EDGE = 5,
1827 EDGE_SCAN = 6
1828
1829 };*/
1830
1831}
1832
1833
1834
1835
1836
1838{
1839 // your periodic code here.
1840 // freq = 15.0 Hz
1841 //printf("Wedgebug periodic function was called\n");
1842
1843
1844 /*
1845 enum control_mode_state {
1846 DIRECT_CONTROL = 1,
1847 AUTONOMOUS_GUIDED = 2,
1848 AUTONOMOUS_NAV = 3
1849 };
1850 */
1851
1852 // Setting control mode state (needed as switch-case statement cannot use OR statment so that
1853 // AP_MODE_ATTITUDE_DIRECT and AP_MODE_ATTITUDE_Z_HOLD lead to the same outcome. Hence I use my
1854 // own states here
1855 switch (autopilot_get_mode()) {
1859 case (AP_MODE_NAV): {current_mode = AUTONOMOUS_NAV;} break;
1860 default: {printf("Unsupported control mode");} break;
1861 }
1862
1863 printf("threshold_median_depth (m) = %f\n", threshold_median_depth / 100.00);
1864
1865 // Debugging - setting default state
1866 //set_state(MOVE_TO_GOAL ,1);
1867 //printf("Current control mode %d\n", current_mode);
1868 //printf("Current state %d\n", current_state);
1869
1870
1871
1872 // Cycle-dependent initializations
1873 //Initialization of dynamic rotation matrices and transition vectors for frame to frame transformations
1874 // Rotation matrix and transition vector to transform from world ned frame to robot frame
1879
1880 // Initialization of robot heading
1882
1883 // Initialization of robot location/orientation-dependent variables
1884 // Converting goal world NED coordinates into Camera coordinates
1886 Va_to_Vb(&VGOALc, &VGOALr, &Rcr, &VCr);
1887
1888
1889 // Checking is state was changed, if yes then all flags are reset and the is_mode_changed_flag
1890 // is set to 1 for this cycle only. Else, the is_mode_changed_flag is set to 0 again;
1891
1892 if (current_mode != previous_mode) {
1893 // Setting flag signifying that the state was changed
1895
1896 } else if (is_mode_changed_flag == 1) {
1898 }
1899
1900 if (is_mode_changed_flag == 1) {printf("Mode was changed!!!!!!!!!!!!!!!!!!!1\n");}
1901 printf("is_mode_changed_flag = %d\n", is_mode_changed_flag);
1902
1904
1905
1906
1907
1908 /*
1909 enum control_mode_state {
1910 DIRECT_CONTROL = 1,
1911 AUTONOMOUS_GUIDED = 2,
1912 AUTONOMOUS_NAV = 3
1913 };
1914 */
1915
1916 /*
1917 enum navigation_state {
1918 MOVE_TO_GOAL = 1,
1919 POSITION_GOAL = 2,
1920 WEDGEBUG_START = 3,
1921 MOVE_TO_EDGE = 4,
1922 POSITION_EDGE = 5,
1923 EDGE_SCAN = 6
1924
1925 };*/
1926 // Finite state machine - Only runs in guided mode
1927 //if ((autopilot_get_mode() == AP_MODE_GUIDED)) // If AP_MODE_GUIDED block - Start
1928
1929 // Switch-case statement for current_mode
1930 switch (current_mode) { // Control mode state machine - Start
1931 // State 1 ################################################################################################################################################################### State 1: DIRECT_CONTROL
1932 case DIRECT_CONTROL: {
1933 printf("DIRECT_CONTROL = %d\n", DIRECT_CONTROL);
1934
1935 // If the mode was just started, then we initialize the FSM with MOVE_TO_GOAL (1)
1938 }
1939
1940 // We do not care about the height of the drone when measuring distance to goal in the DIRECT_CONTROL mode
1941 // So we create this pseudo 2d vector where the z coordinates are the same as the goal. This vector is used
1942 // to check if robot is close to goal
1943 struct FloatVect3 VR2dwned = {.x = VRwned.x, .y = VRwned.y, .z = VGOALwned.z};
1944
1945
1946 // Background processes - Includes image processing for use
1947 // Turn on for debugging
1948 //background_processes(save_images_flag);
1950
1951
1952 switch (current_state) { // Finite state machine - Start
1953 // State 1 --------------------------------------------------------------------------------------------------------------------------------------------------------------- State 1: MOVE_TO_GOAL
1954 case MOVE_TO_GOAL: {
1955 printf("MOVE_TO_GOAL = %d\n", MOVE_TO_GOAL);
1956 //median_disparity_in_front = median_disparity_to_point(&c_img_cropped, &img_disparity_int8_cropped, &median_kernel);
1957 //In case disparity is 0 (infinite distance or error we set it to one disparity
1958 // above the threshold as the likelihood that the object is too close is large (as opposed to it being infinitely far away)
1959 //printf("median_disparity_in_front = %d\n", median_disparity_in_front);
1960 //printf("median_depth_in_front = %f\n", disp_to_depth(median_disparity_in_front, b, f));
1961 //float depth = disp_to_depth(median_disparity_in_front, b, f); // median_depth_in_front / 100.00
1962
1963
1965 float depth = median_depth_in_front / 100.00;
1966 printf("median_depth_in_front = %f\n", depth);
1967 printf("depth to goal = %f\n", VGOALc.z);
1968
1969
1971 printf("Obstacle is in front of goal\n");
1972 } else {
1973 printf("The path to the goal is free!\n");
1974 }
1975
1976
1980 &VGOALc,//target_point,
1986
1987
1988// // c. Checking if edges are located
1989// // Running function to detect and save edge
1990// is_edge_found_micro_flag =
1991// find_best_edge_coordinates(
1992// &VEDGECOORDINATESc,
1993// &VGOALc,//target_point,
1994// &img_edges_int8_cropped,
1995// &img_middle_int8_cropped,
1996// &edge_search_area,
1997// threshold_disparity_of_edges,
1998// max_edge_found_micro_confidence);
1999//
2000
2001
2002
2003 if (save_images_flag) {save_image_gray(&img_edges_int8_cropped, "/home/dureade/Documents/paparazzi_images/img_edges_int8_cropped_marked.bmp");}
2004
2005
2007 printf("Goal is reached\n");
2009 } else {
2010 // If this is the first cycle of this mode, then
2013 }
2014
2015 // ############ Metric 2 - Distance traveled (total)
2020
2021 // If the Goal is reached, set is_setpoint_reached_flag to 1 and record time
2025
2026 }
2027 }
2028 } break;
2029
2030
2031 // State 2 --------------------------------------------------------------------------------------------------------------------------------------------------------------- State 2: POSITION_GOAL
2032 case POSITION_GOAL: {
2033 printf("POSITION_GOAL = %d\n", POSITION_GOAL);
2034 printf("Total time to reach goal = %f\n", ((double)clock_total_time) / CLOCKS_PER_SEC);
2035 printf("Total distance_traveled = %f\n", distance_traveled);
2036
2037 } break;
2038
2039
2040 // State 0 --------------------------------------------------------------------------------------------------------------------------------------------------------------- State 0
2041 default: {
2042 printf("default = %d\n", 0);
2043 }
2044 } // Finite state machine - End
2045
2046
2047 printf("Time elapsed since start = %f\n", (((double)clock()) - ((double)clock_total_time_current)) / CLOCKS_PER_SEC);
2048 printf("distance_traveled = %f\n", distance_traveled);
2049
2050 } break; // DIRECT_CONTROL - End
2051
2052
2053 // State 2 ################################################################################################################################################################### State 2: AUTONOMOUS_GUIDED
2054 case AUTONOMOUS_GUIDED: {
2055 printf("AUTONOMOUS_GUIDED = %d\n", AUTONOMOUS_GUIDED);
2056
2057 // Checking is state was changed, if yes then all flags are reset and the is_state_changed_flag
2058 // is set to 1 for this cycle only. Else, the is_state_changed_flag is set to 0 again;
2060 // Setting flag signifying that the state was changed
2062 // Reset flags
2063 is_start_reached_flag = 0; // Set to 1 if start position is reached, 0 otherwise
2064 is_setpoint_reached_flag = 0; // Set to 1 if setpoint is reached, 0 otherwise
2065 is_obstacle_detected_flag = 0; // Set to 1 if obstacle is detected, 0 otherwise
2066 is_path_free_flag = 0; // Set to 1 if no obstacle is detected, 0 otherwise
2067 is_heading_reached_flag = 0; // Set to 1 if heading is reached, 0 otherwise
2068 is_edge_found_macro_flag = 0; // Set to 1 if best edge (according to macro confidence) was found, 0 otherwise
2069 is_edge_found_micro_flag = 0; // Set to 1 if best edge (according to micro confidence) was found, 0 otherwise
2070 is_no_edge_found_flag = 0; // Set to 1 if no edge was identified, 0 otherwise
2071 initial_heading.initiated = 0; // 0 = it can be overwritten
2072 initial_heading.is_left_reached_flag = 0;// The scan has not reached the left maximum angle yet
2073 initial_heading.is_right_reached_flag = 0;// The scan has not reached the right maximum angle yet
2074 distance_robot_edge_goal = 99999; //
2075 } else if (is_state_changed_flag != 0) {
2077 }
2078
2079 // Background processes only happens if the current state is not POSITION_GOAL
2081 // ############ Metric 1 - Recording current time
2083 //printf("clock_total_time_current = %f\n", (double)clock_total_time_current);
2084 // In case we are in the position start state and the state has changed, initialize clock_total_time_previous
2086 printf("Metric 1 was started\n");
2087 //printf("clock_total_time_current set = %f\n", (double)clock_total_time_current);
2089 }
2090 //Else check time difference to previous cycle and add to clock_total_time
2091 else {
2094 //printf("clock_total_time_previous = %f\n", (double)clock_total_time_previous);
2095 }
2096
2097 // ############ Metric 2 - Distance traveled (total)
2099
2103
2104 // ############ Metric 3 - Runtime average of background processes (see below) - Start:
2105 clock_t time; // Creating variable to hold time (number of cycles)
2106 time = clock(); // Saving current time, way below it is used to calculate time spent in a cycle
2107 counter_cycles++; // Counting how many times a state was activated (needed for average calculation)
2108
2109 // Background processes - Includes image processing for use
2110 //background_processes(save_images_flag);
2112
2113 // ############ Metric 3 - Runtime average of background processes (see below) - End:
2115
2116 // ############ Metric 4 - Runtime average per state - Start:
2117 clock_FSM = clock(); // Saving current time, way below it is used to calculate time spent in a cycle
2118 counter_state[current_state]++; // Counting how many times a state was activated (needed for average calculation). This is done here as state may change in FSM
2119 }
2120
2121 // Initializing previous_state variable for next cycle
2122 // This happens here and not above as the metric above depends on the previous state
2124
2125 switch (current_state) { // Finite state machine - Start
2126 // State 1 --------------------------------------------------------------------------------------------------------------------------------------------------------------- State 1: MOVE_TO_GOAL
2127 case MOVE_TO_GOAL: {
2128 printf("MOVE_TO_GOAL = %d\n", MOVE_TO_GOAL);
2129 // 1. Moving robot towards the goal
2130 // Checking if goal is reached, if not continue to move
2132 printf("Goal is reached\n");
2134
2135 }
2136 // Else, move to goal and check confidence that goal has been reached.
2137 else {
2138 // Sets setpoint to goal position and orientates drones towards the goal as well
2140
2141
2142 // If start appears to be reached increase confidence
2146 }
2147
2148 // If the position_confidence is high enough, set is_start_reached_flag to 1 and reset position_confidence
2152 }
2153 }
2154
2155
2156 // 2. Check if obstacle is in way, when one is detected change state (dependent on negative flag from 1)
2157 // If the setpoint has not been reached, keep looking for obstacles
2159 // Checking if obstacle is in way, if not continue checking for it
2161 printf("Object detected!!!!!!!!\n");
2162 // 1. Seting setpoint to current location
2165 }
2166 // Else, check confidence that obstacle is there
2167 else { // This happens continuously, as long as the state is active and the goal has not been reached. It stops when the flag has been set below.
2168 // Calculate median depth in front
2170 float depth = median_depth_in_front / 100.00;
2171 printf("median_depth_in_front = %f\n", depth);
2172 printf("depth to goal = %f\n", VGOALc.z);
2173
2174
2175 // If obstacle appears to be detected AND its in front of goal point, increase obstacle_confidence
2177 && (depth < VGOALc.z)) { // NOTE. The first logical statement is in centimeters and the second in meters
2178 printf("Increasing confidence\n");
2181 printf("obstacle_confidence = %d\n", obstacle_confidence);
2182 printf("max_obstacle_confidence = %d\n", max_obstacle_confidence);
2183 } else {
2185 }
2186 // If the obstacle_confidence is high enough, set is_obstacle_detected_flag to 1 and reset obstacle_confidence
2190 // Setting holding point so that when state changes to wedgebug the drone stays at one spot, when looking for edges
2194 }
2195 }
2196 }
2197
2198 printf("position_confidence = %d\n", position_confidence);
2199 printf("obstacle_confidence = %d\n", obstacle_confidence);
2200 } break;
2201
2202
2203 // State 2 --------------------------------------------------------------------------------------------------------------------------------------------------------------- State 2: POSITION_GOAL
2204 case POSITION_GOAL: {
2205 printf("POSITION_GOAL = %d\n", POSITION_GOAL);
2206 // Since the drone is at the goal we will swithc bach to the NAV mode
2207 //autopilot_mode_auto2 = AP_MODE_NAV;
2208 //autopilot_static_set_mode(AP_MODE_NAV);
2209
2210 printf("Total time to reach goal = %f\n", ((double)clock_total_time) / CLOCKS_PER_SEC);
2211 printf("Total distance_traveled = %f\n", distance_traveled);
2212 printf("Average runtime of background processes = %f\n",
2214
2215 for (uint8_t i = 0; i < number_of_states; i++) {
2216 printf("Average runtime of state %d = %f\n", i, (time_state[i] / CLOCKS_PER_SEC / counter_state[i]));
2217 }
2218 } break;
2219
2220
2221 // State 3 --------------------------------------------------------------------------------------------------------------------------------------------------------------- State 3: WEDGEBUG_START
2222 case WEDGEBUG_START: {
2223 printf("WEDGEBUG_START = %d\n", WEDGEBUG_START);
2224 /* Details:
2225 * 1) The robot checks if holding point was reached, at which it first detected that an obstacle was it its way
2226 * 2) If that holding point is reached, the robot then scans for any edges.
2227 *
2228 */
2229
2230 // 1. Move robot towards holding point
2231 // Checking if holding point is reached, if not continue to move
2233 printf("Holding point is reached\n");
2234 }
2235 // Else, move to holding point and check confidence that holding point has been reached.
2236 else { // This happens continuously, as long as the state is active. It stops when the flag has been set below.
2237
2238 // Sets setpoint to goal position and orientates drones towards the goal as well
2241
2242 // If holding point appears to be reached increase confidence
2246 }
2247
2248 // If the position_confidence is high enough, set is_setpoint_reached_flag to 1 and reset position_confidence
2252 }
2253 }
2254
2255
2256 // 2. Scan for edges and see if any exist or not, then change state (dependent on flag from 1).
2258 // a. Stopping drone movement (a second time, just to make sure)
2260
2261 // b. Checking if edges are located
2262 // Running function to detect and save edge
2266 &VGOALc,//target_point,
2272
2273
2274 //printf("was_edge_found?: %d\n", was_edge_found);
2275
2276
2277 // d. In this if-else statement we check if an edge was detected. If no edge is detected (by find_best_edge_coordinates) no_edge_found_confidence is
2278 // increased, whereas if an edge is found the edge_found_macro_confidence is increased. If one of the confidences reaches the
2279 // maximum the respective flag is turned on.
2280 // If obstacle is in way
2283 }
2284 // Else if no edge is detected
2285 else if (is_no_edge_found_flag) {
2286 printf("Edge not found!!!!!!!!!!!!!!!!!\n");
2288 }
2289 // Else, check if micro edge has been detected (by find_best_edge_coordinates) or not and increase respective confidences.
2290 // Also, check if macro confidence of having detected an edge is reached or whether maximum confidence of having no edges
2291 // detected is reached. If any are reached, respective flags are set
2292 else {
2293 // This if statement increase the confidence
2294 // If edge is detected in find_best_edge_coordinates, increase edge_found_macro_confidence
2298 }
2299 // If no edge is detected in find_best_edge_coordinates, increase no_edge_found_confidence
2300 else {
2303 }
2304
2305 // If the edge_found_macro_confidence is high enough, set is_edge_found_macro_flag to 1 and reset edge_found_macro_confidence and no_edge_found_confidence
2310
2311 // Since we are sure that a suitable edge has been found, the camera coordinates are transformed to
2312 // world coordinates and saved in a global variable, for used in subsequent state to make drone move
2313 // towards this point
2317 //printf("Optimal edge found\n");
2318 //printf("Camera coordinates of edge: [%f, %f, %f]\n", VEDGECOORDINATESc.x, VEDGECOORDINATESc.y, VEDGECOORDINATESc.z);
2319 // Saving best edge in global variable. // Note. VPBESTEDGECOORDINATESwned is used in the MOVE_TO_EDGE (7) and POSITION_EDGE (8) states
2323
2324 // Making snapshot of image with edge coordinates highlighted. Comment out if not needed
2325 if (save_images_flag) {save_image_gray(&img_edges_int8_cropped, "/home/dureade/Documents/paparazzi_images/img_edges_int8_cropped_marked.bmp");}
2326
2327 }
2328 // If the no_edge_found_macro_confidence is high enough, set is_no_edge_found_macro_flag to 1 and reset edge_found_macro_confidence and no_edge_found_confidence
2333 }
2334 }
2335 printf("Edge found confidence = %d\n", edge_found_macro_confidence);
2336 printf("No Edge found confidence = %d\n", no_edge_found_confidence);
2337 }
2338 } break;
2339
2340
2341 // State 4 --------------------------------------------------------------------------------------------------------------------------------------------------------------- State 4: MOVE_TO_EDGE
2342 case MOVE_TO_EDGE: {
2343 printf("MOVE_TO_EDGE = %d\n", MOVE_TO_EDGE);
2344
2345 //printf("Camera coordinates of edge setpoint = [%f, %f, %f]\n", VEDGECOORDINATESc.x, VEDGECOORDINATESc.y, VEDGECOORDINATESc.z);
2346 //printf("Robot coordinates of edge setpoint = [%f, %f, %f]\n", VEDGECOORDINATESr.x, VEDGECOORDINATESr.y, VEDGECOORDINATESr.z);
2347 printf("World NED coordinates of edge setpoint = [%f, %f, %f]\n", VPBESTEDGECOORDINATESwned.x,
2349 //printf("World ENU coordinates of edge setpoint = [%f, %f, %f]\n", VEDGECOORDINATESwenu.x, VEDGECOORDINATESwenu.y, VEDGECOORDINATESwenu.z);
2350
2351
2352 // 1. Orientate robot towards object edge detected
2353 // If correct angle is reached, nothing happens
2355 printf("Heading is reached\n");
2356 }
2357 // Else, adjust heading and check confidence that heading is reached
2358 else { // This happens continuously, as long as the state is active. It stops when the flag has been set below
2359 // Set desired heading
2361 &VPBESTEDGECOORDINATESwned));// heading_towards_waypoint(WP_GOAL));
2362
2363 // If heading appears to be reached increase confidence
2368 }
2369
2370 // If the heading_confidence is high enough, set is_heading_reached_flag to 1 and reset heading_confidence
2374 }
2375 }
2376
2377
2378 // 2.Move robot to edge detected (dependent on flag from 1)
2379 // If the robot faces the right direction, go to the object edge coordinates (NEW) detected
2381 // If edge setpoint is reached, stay at it
2383 printf("Edge is reached\n");
2386 }
2387 // Else, move to edge setpoint and check confidence that edge setpoint is reached
2388 else { // This happens continuously, as long as the state is active. It stops when the flag has been set below.
2389
2390 // Sets setpoint to edge position and orientates robot towards it as well
2393
2394 // If edge appears to be reached increase confidence
2396 threshold_distance_to_goal)) { // && !is_setpoint_reached_flag)
2399 }
2400
2401 // If the position_confidence is high enough, set is_setpoint_reached_flag to 1 and reset position_confidence
2405 }
2406 }
2407 }
2408
2409
2410
2411 // 3. Set next state (dependent on 1 and 2)
2412 // If the robot faces the correct angle AND the edge is then reached, the state is changed to "MOVE_TO_GOAL"
2414 printf("Position and Heading are reached\n");
2416
2417 }
2418 printf("position_confidence = %d\n", position_confidence);
2419 printf("heading_confidence = %d\n", heading_confidence);
2420 } break;
2421
2422
2423 // State 5 --------------------------------------------------------------------------------------------------------------------------------------------------------------- State 5: POSITION_EDGE
2424 case POSITION_EDGE: {
2425 printf("POSITION_EDGE = %d\n", POSITION_EDGE);
2426
2427 // This ensures that the robot stay on the edge at all times
2430
2431 // 1. Orientates robot towards the final goal
2432 // If robot faces goal, robot stays on current edge
2434 printf("Heading is reached\n");
2436 }
2437 // Else, adjust heading and check confidence that heading is reached
2438 else { // This happens continuously, as long as the state is active. It stops when the flag has been set.
2439 // Set desired heading
2441
2442 // If heading appears to be reached increase confidence
2446 }
2447
2448 // If the heading_confidence is high enough, set is_heading_reached_flag to 1 and reset heading_confidence
2452 }
2453 }
2454
2455
2456 // 2. Check if object is blocking the path or not and set next state (dependent on flag from 1)
2457 // If the robot faces the right direction, check if obstacle exists or not and change state
2459 // In this if-else statement we check if an obstacle is blocking the path or not. If no obstacle is blocking the path the free_path_confidence is
2460 // increased, whereas if an obstacle is blocking the path the obstacle_confidence is increased. If one of the confidences reaches the
2461 // maximum the respective flag is turned on. These statements only execute if the robot faces the goal (see above)
2462 // If obstacle is in way
2464 printf("Object detected!!!!!!!!\n");
2465 // Setting new holding point for WEDGEBUG_START state
2470 }
2471 // Else if the path is clear
2472 else if (is_path_free_flag) {
2473 printf("Path is free\n");
2475 }
2476 // Else, check for obstacles and free path and check confidence that an obstacle is there or that the path is free
2477 else {
2478 // Calculate median depth in front
2480 &median_kernel16bit); // Median depth in centimeters (cm)
2481 printf("Median_depth_in_front (m) = %f\n", (median_depth_in_front / 100.00)); // Median depth in meters (m)
2482 printf("Depth to goal = %f\n", VGOALc.z); // Depth in meters (m)
2483
2484 // This if-else statement increase the confidence
2485 // If obstacle appears to be detected AND its in front of goal point, increase obstacle_confidence
2487 && ((median_depth_in_front / 100.00) <
2488 VGOALc.z)) { // NOTE. The first logical statement is in centimeters and the second in meters
2492
2493 }
2494 // If obstacle appears not detected, increase free_path_confidence
2495 else {
2499 }
2500 // If the obstacle_confidence is high enough, set is_obstacle_detected_flag to 1 and reset obstacle_confidence and free_path_confidence
2505 }
2506 // If the free_path_confidence is high enough, set is_path_free_flag to 1 and reset free_path_confidence and obstacle_confidence
2511 }
2512 }
2513
2514 }
2515 printf("Obstacle confidence = %d\n", obstacle_confidence);
2516 printf("Free path confidence = %d\n", free_path_confidence);
2517 } break;
2518
2519
2520 // State 6 --------------------------------------------------------------------------------------------------------------------------------------------------------------- State 6: EDGE_SCAN
2521 case EDGE_SCAN: {
2522 printf("EDGE_SCAN = %d\n", EDGE_SCAN);
2523
2524 // 1. The robot is tasked to stay at the holding point for the entire duration of this state
2525 // Making drone hover, so that it does not drift from its current position
2527
2528 // 2. Checking if edges are located
2529 // Running function to detect and save edge
2533 &VGOALc,//target_point,
2539
2540
2541 // 3. The identified best edge is compared to the previous identified best edge, and replace if the total distance (robot-edge-goal)
2542 // has decreased (dependent on flag from 2)
2543 // If edge was found
2545 // First we convert edge camera coordinates into world coordinates (ned and enu)
2546 //VEDGECOORDINATESc.z = VEDGECOORDINATESc.z - 0.25;// add this to make sure drone does not exactly stop on the edge point identified (it might crash otherwise)
2550
2551 // Second, we increase confidence that an edge was spotted
2553
2554 // Third, we calculate the distance of the edge currently spotted, if distance is smaller
2555 // than previous distance VPBESTEDGECOORDINATESwned is updated with the coordinates
2556 // Note. VPBESTEDGECOORDINATESwned is used in the MOVE_TO_EDGE (7) and POSITION_EDGE (8) states
2557 // Calculating Euclidean distance (N2) - Edge to goal
2559 // Calculating Euclidean distance (N2) - robot to edge
2562 printf("distance_total =%f\n", distance_total);
2568 }
2569 }
2570
2571 printf("Current minimum distance =%f\n", distance_robot_edge_goal);
2572 printf("is_edge_found_micro_flag = %d\n", is_edge_found_micro_flag);
2573
2574
2575 // 4. Initializing current heading parameters (if they have not been initialized before)
2576 if (initial_heading.initiated == 0) {
2579 2); // this way, when the the center of the drone is facing left, its fov does not exceed the maximum angle to the left
2582 2); // this way, when the the center of the drone is facing right, its fov does not exceed the maximum angle to the right
2585 }
2586
2587
2588 // 5. The drone turns right to find any edges
2589 // Code for looking left
2590 // If left heading has been reached (i.e. if the flag is activated)
2592 printf("Left heading is/was reached\n");
2593 }
2594 // Else, adjust angle to to face more left and check confidence that left heading has been reached
2595 else { // This happens continuously, as long as the state is active. It stops when the flag has been set below
2596 // Set heading to maximum left heading
2598
2599 // If heading appears to be reached increase confidence
2603 }
2604 // If the heading_confidence is high enough, set initial_heading.is_left_reached_flag to 1 and reset heading_confidence
2608 }
2609 }
2610
2611
2612
2613 // 6. The drone turns right to find any edges (dependent on flag from 5)
2614 // Code for looking right - Only runs if robot has previously looked left (initial_heading.heading_max_left = 1)
2616 // If right heading has been reached (i.e. if the flag is activated)
2618 printf("Right heading is/was reached\n");
2619 }
2620 // Else, adjust angle to to face more right and check confidence that right heading has been reached
2621 else {
2622 // Set heading to maximum left heading
2624
2625 // If heading appears to be reached increase confidence
2629 }
2630 // If the heading_confidence is high enough, set initial_heading.is_right_reached_flag to 1 and resets heading_confidence
2634 }
2635 }
2636 }
2637
2638
2639 // 7. Check if confidence of having found a suitable edge is large enough or not and set next state (dependent on flags from 5 and 6)
2642 printf("Edge has been found\n");
2644 } else if (is_no_edge_found_flag) {
2645 printf("Minimum has been encountered\n");
2646 } else {
2647
2648 // If edge_found_macro_confidence is >= max_edge_found_macro_confidence then set is_edge_found_macro_flag
2649 // and reset edge_found_macro_confidence
2653 }
2654 // Else, set is_no_edge_found_flag and reset edge_found_macro_confidence
2655 else {
2658 }
2659
2660 }
2661 }
2662
2663 printf("heading_confidence = %d\n", heading_confidence);
2664 printf("edge_found_confidence = %d\n", edge_found_macro_confidence);
2665 } break;
2666
2667 // State 0 --------------------------------------------------------------------------------------------------------------------------------------------------------------- State 0
2668 default: {
2669 printf("default = %d\n", 0);
2670 }
2671 break;
2672
2673 } // Finite state machine - End
2674
2675
2676
2677 // ############ Metric 4 - Runtime average per state - End:
2678 clock_FSM = clock() - clock_FSM; // Calculating time it took for the FSM to finish running
2679 time_state[current_state] += ((double)clock_FSM); // Adding calculated time to total time of current state
2680
2681 printf("Time elapsed since start = %f\n", ((double)clock_total_time) / CLOCKS_PER_SEC);
2682 printf("distance_traveled = %f\n", distance_traveled);
2683 } break; // AUTONOMOUS_GUIDED - End
2684
2685
2686 // State 3 ################################################################################################################################################################### State 3: AUTONOMOUS_NAV
2687 case AUTONOMOUS_NAV: {
2688 printf("AUTONOMOUS_NAV = %d\n", AUTONOMOUS_NAV);
2689 // Background processes - Includes image processing for use
2691
2692
2694 float depth = median_depth_in_front / 100.00;
2695 printf("median_depth_in_front = %f\n", depth);
2696 printf("depth to goal = %f\n", VGOALc.z);
2697
2698
2700 printf("Obstacle is in front of goal\n");
2701 } else {
2702 printf("The path to the goal is free!\n");
2703 }
2704
2705
2709 &VGOALc,//target_point,
2715
2716
2717// // c. Checking if edges are located
2718// // Running function to detect and save edge
2719// is_edge_found_micro_flag =
2720// find_best_edge_coordinates(
2721// &VEDGECOORDINATESc,
2722// &VGOALc,//target_point,
2723// &img_edges_int8_cropped,
2724// &img_middle_int8_cropped,
2725// &edge_search_area,
2726// threshold_disparity_of_edges,
2727// max_edge_found_micro_confidence);
2728//
2729
2730
2731
2732 if (save_images_flag) {save_image_gray(&img_edges_int8_cropped, "/home/dureade/Documents/paparazzi_images/img_edges_int8_cropped_marked.bmp");}
2733
2734
2735
2736 } break; // AUTONOMOUS_NAV - End
2737
2738
2739
2740 // State 0 ################################################################################################################################################################### State 0
2741 default:
2742 {printf("Unsupported control mode");} break;
2743
2744 } // Control mode state machine - End
2745
2746
2747
2748
2749 /*
2750 // printing flags
2751 printf("is_start_reached_flag = %d\n", is_start_reached_flag);
2752 printf("is_setpoint_reached_flag = %d\n", is_setpoint_reached_flag);
2753 printf("is_obstacle_detected_flag = %d\n", is_obstacle_detected_flag);
2754 printf("is_path_free_flag = %d\n", is_path_free_flag);
2755 printf("is_heading_reached_flag = %d\n", is_heading_reached_flag);
2756 printf("is_state_changed_flag = %d\n", is_state_changed_flag);
2757 printf("initial_heading.initiated = %d\n", initial_heading.initiated);
2758 printf("initial_heading.is_left_reached_flag = %d\n", initial_heading.is_left_reached_flag);
2759 printf("initial_heading.is_right_reached_flag = %d\n", initial_heading.is_right_reached_flag);
2760
2761 */
2762 printf("\n\n");
2763
2764
2765
2766 save_image_gray(&img_left_int8, "/home/dureade/Documents/paparazzi_images/img_left_int8.bmp");
2767 save_image_gray(&img_right_int8, "/home/dureade/Documents/paparazzi_images/img_right_int8.bmp");
2768 save_image_HM(&img_disparity_int8_cropped, "/home/dureade/Documents/paparazzi_images/img_disparity_int8_cropped.bmp",
2770 //save_image_gray(&img_left_int8_cropped, "/home/dureade/Documents/paparazzi_images/img_left_int8_cropped.bmp");
2771 save_image_HM(&img_middle_int8_cropped, "/home/dureade/Documents/paparazzi_images/img_intermediate_int8_cropped.bmp",
2773 save_image_gray(&img_edges_int8_cropped, "/home/dureade/Documents/paparazzi_images/img_edges_int8_cropped.bmp");
2774
2775
2776
2777 /*
2778 // Size of variables
2779 printf("img_left = %d\n", img_left.buf_size);
2780 printf("img_right = %d\n", img_right.buf_size);
2781 printf("img_left_int8 = %d\n", img_left_int8.buf_size);
2782 printf("img_left_int8_cropped = %d\n", img_left_int8_cropped.buf_size);
2783 printf("img_right_int8 = %d\n", img_right_int8.buf_size);
2784 printf("img_disparity_int8_cropped = %d\n", img_disparity_int8_cropped.buf_size);
2785 printf("img_edges_int8_cropped = %d\n", img_edges_int8_cropped.buf_size);
2786 printf("median_kernel = %d\n", median_kernel.buf_size);
2787 printf("SE_opening_OCV = %lu\n", sizeof(SE_opening_OCV));
2788 printf("VSTARTwenu.x = %lu\n", sizeof(VSTARTwenu.x));
2789 printf("current_state = %lu\n", sizeof(current_state));
2790 printf("current_mode = %lu\n", sizeof(current_mode));
2791 */
2792
2793
2794
2795
2796
2797
2798
2799
2800 /*
2801 * Heat maps:
2802 * 0 = COLORMAP_AUTUMN
2803 * 1 = COLORMAP_BONE
2804 * 2 = COLORMAP_JET
2805 * 3 = COLORMAP_WINTER
2806 * 4 = COLORMAP_RAINBOW
2807 * 5 = COLORMAP_OCEAN
2808 * 6 = COLORMAP_SUMMER
2809 * 7 = COLORMAP_SPRING
2810 * 8 = COLORMAP_COOL
2811 * 9 = COLORMAP_HSV
2812 * 10 = COLORMAP_PINK
2813 * 11 = COLORMAP_HOT
2814 */
2815
2816
2817}
2818
2819
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition autopilot.c:222
Core autopilot interface common to all firmwares.
bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
Set position and heading setpoints in GUIDED mode.
Autopilot guided mode interface.
#define WaypointX(_wp)
Definition common_nav.h:45
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition common_nav.h:48
#define WaypointY(_wp)
Definition common_nav.h:46
struct video_listener * cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps, uint8_t id)
Definition cv.c:46
Computer vision framework for onboard processing.
float m[3 *3]
float psi
in radians
#define FLOAT_ANGLE_NORMALIZE(_a)
static void float_vect_copy(float *a, const float *b, const int n)
a = b
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
static float float_vect3_norm(struct FloatVect3 *v)
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
rotation matrix
#define VECT2_DIFF(_c, _a, _b)
#define VECT3_DIFF(_c, _a, _b)
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition state.h:1300
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1306
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:848
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition state.h:839
void image_copy(struct image_t *input, struct image_t *output)
Copy an image from inut to output This will only work if the formats are the same.
Definition image.c:89
void image_to_grayscale(struct image_t *input, struct image_t *output)
Convert an image to grayscale.
Definition image.c:131
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
Definition image.c:43
Image helper functions like resizing, color filter, converters...
uint16_t h
height of the cropped area
Definition image.h:96
uint16_t h
The height.
Definition image.h:88
uint16_t w
Width of the cropped area.
Definition image.h:95
uint32_t x
The x coordinate of the point.
Definition image.h:59
uint16_t x
Start position x (horizontal)
Definition image.h:93
uint16_t y
Start position y (vertical)
Definition image.h:94
uint32_t y
The y coordinate of the point.
Definition image.h:60
uint16_t h
Image height.
Definition image.h:47
uint16_t w
Image width.
Definition image.h:46
enum image_type type
The image type.
Definition image.h:45
image_type
Definition image.h:35
@ IMAGE_GRAYSCALE
Grayscale image with only the Y part (uint8 per pixel)
Definition image.h:37
@ IMAGE_YUV422
UYVY format (uint16 per pixel)
Definition image.h:36
@ IMAGE_GRADIENT
An image gradient (int16 per pixel)
Definition image.h:39
@ IMAGE_INT16
An image to hold disparity image data from openCV (int16 per pixel)
Definition image.h:40
@ IMAGE_JPEG
An JPEG encoded image (not per pixel encoded)
Definition image.h:38
uint16_t w
The width.
Definition image.h:87
Definition image.h:92
uint16_t foo
Definition main_demo5.c:58
Paparazzi generic algebra macros.
Paparazzi floating point algebra.
Paparazzi floating point math for geodetic calculations.
float z
in meters
float x
in meters
float y
in meters
#define AP_MODE_ATTITUDE_DIRECT
#define AP_MODE_NAV
#define AP_MODE_GUIDED
#define AP_MODE_ATTITUDE_Z_HOLD
void guidance_h_set_pos(float x, float y)
Set horizontal position setpoint.
Definition guidance_h.c:455
void guidance_h_hover_enter(void)
Definition guidance_h.c:343
void guidance_h_set_heading(float heading)
Set heading setpoint.
Definition guidance_h.c:465
void guidance_v_set_z(float z)
Set z position setpoint.
Definition guidance_v.c:368
API to get/set the generic vehicle states.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
float max_edge_search_angle
Variable for storing the heading of the drone (psi in radians)
Definition wedgebug.c:259
uint16_t K_median_h
Width of kernel for the median kernel.
Definition wedgebug.c:113
struct FloatVect3 VHOLDINGPOINTwned
Declared a vector to hold the current position, which is needed for calculating the distance traveled...
Definition wedgebug.c:135
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition wedgebug.c:204
void set_state(uint8_t _state, uint8_t change_allowed)
Definition wedgebug.c:420
uint16_t threshold_median_depth
Below this distance (in meters) it is considered that the robot has reached the goal,...
Definition wedgebug.c:149
void Vi_to_Vc16bit(struct FloatVect3 *scene_point, int32_t image_point_y, int32_t image_point_x, const uint16_t d, const float b, const uint16_t f)
Definition wedgebug.c:774
float disp_to_depth_16bit(const int16_t d, const float b, const uint16_t f)
Definition wedgebug.c:705
uint8_t allow_state_change_POSITION_EDGE
Definition wedgebug.c:275
void wedgebug_init()
Definition wedgebug.c:1584
uint8_t is_obstacle_detected_flag
Set to 1 if setpoint is reached, 0 otherwise.
Definition wedgebug.c:172
#define WEDGEBUG_CAMERA_LEFT_FPS
Definition wedgebug.c:65
uint8_t is_setpoint_reached(struct FloatVect3 *VGOAL, struct FloatVect3 *VCURRENTPOSITION, float threshold)
Definition wedgebug.c:1348
enum control_mode_state current_mode
Definition wedgebug.c:226
struct point_t c_img
Definition wedgebug.c:183
uint8_t is_no_edge_found_flag
Set to 1 if best edge (according to micro confidence) was found, 0 otherwise.
Definition wedgebug.c:177
uint8_t is_mode_changed_flag
Set to 1 if state was changed, 0 otherwise.
Definition wedgebug.c:179
uint8_t find_best_edge_coordinates2(struct FloatVect3 *VEDGECOORDINATESc, struct FloatVect3 *VTARGETc, struct image_t *img_edges, struct image_t *img_depth, struct crop_t *edge_search_area, uint16_t threshold, int16_t max_confidence)
Definition wedgebug.c:1233
int16_t max_heading_confidence
This is the max confidence that a specific position was reached.
Definition wedgebug.c:164
float threshold_distance_to_angle
Below this distance (in meters) it is considered that the robot has reached the goal.
Definition wedgebug.c:146
float float_norm_two_angles(float target_angle, float current_angle)
Definition wedgebug.c:1356
navigation_state
Camera focal length, in pixels (i.e. distance between camera.
Definition wedgebug.c:209
@ EDGE_SCAN
Definition wedgebug.c:215
@ WEDGEBUG_START
Definition wedgebug.c:212
@ MOVE_TO_GOAL
Definition wedgebug.c:210
@ POSITION_GOAL
Definition wedgebug.c:211
@ POSITION_EDGE
Definition wedgebug.c:214
@ MOVE_TO_EDGE
Definition wedgebug.c:213
uint8_t allow_state_change_EDGE_SCAN
Definition wedgebug.c:276
uint8_t is_heading_reached(float target_angle, float current_angle, float threshold)
Definition wedgebug.c:1367
struct FloatVect3 VGOALwenu
Declared vector of coordinates of start position in NED world coordinate system.
Definition wedgebug.c:121
struct point_t c_img_cropped
Principal point of normal camera images.
Definition wedgebug.c:184
uint8_t previous_state
Definition wedgebug.c:253
struct img_size_t kernel_median_dims
Dimension of image after it has been cropped to remove pixel error due to block matching limitations.
Definition wedgebug.c:98
void disp_to_depth_img(struct image_t *img8bit_input, struct image_t *img16bit_output)
Definition wedgebug.c:1385
struct image_t img_left
Definition wedgebug.c:74
struct img_size_t img_cropped_dims
Dimensions of images captured by the camera (left and right have same dimension)
Definition wedgebug.c:96
int heat_map_type
Variable to hold distance from robot to edge to goal (used in EDGE_SCAN (9) state)
Definition wedgebug.c:266
int SE_closing_OCV
SE size for the opening operation.
Definition wedgebug.c:107
double time_state[NUMBER_OF_STATES]
Definition wedgebug.c:243
struct FloatVect3 VPBESTEDGECOORDINATESwned
Declared a vector to hold the position of a holding point (offten used to make sure drone stays still...
Definition wedgebug.c:137
void wedgebug_periodic()
Definition wedgebug.c:1837
void background_processes(uint8_t save_images_flag)
Definition wedgebug.c:1427
void show_rotation_matrix(struct FloatRMat *R)
Definition wedgebug.c:387
struct image_t img_disparity_int8_cropped
Image obtained from right camera, converted into 8bit gray image.
Definition wedgebug.c:79
int16_t max_edge_found_macro_confidence
This is the max confidence that edges (micro-see above) were found.
Definition wedgebug.c:166
clock_t clock_total_time
Definition wedgebug.c:246
int32_t indx1d_a(const int32_t y, const int32_t x, const struct image_t *img)
Definition wedgebug.c:807
float distance_robot_edge_goal
Definition wedgebug.c:265
clock_t clock_background_processes
Definition wedgebug.c:249
uint8_t is_edge_found_micro_flag
Set to 1 if best edge (according to macro confidence) was found, 0 otherwise.
Definition wedgebug.c:176
uint32_t maximum_intensity(struct image_t *img)
Definition wedgebug.c:633
struct FloatRMat Rcr
Definition wedgebug.c:198
void Va_to_Vb(struct FloatVect3 *Vb, struct FloatVect3 *Va, struct FloatRMat *Rba, struct FloatVect3 *VOa)
Definition wedgebug.c:851
float heading_max_left
Definition wedgebug.c:233
float heading_towards_setpoint_WNED(struct FloatVect3 *VSETPOINTwned)
Definition wedgebug.c:991
uint8_t allow_state_change_POSITION_GOAL
Definition wedgebug.c:272
uint16_t getMedian16bit(uint16_t *a, uint32_t n)
Definition wedgebug.c:500
int16_t max_position_confidence
This is the max confidence that an obstacle was not spotted.
Definition wedgebug.c:163
int16_t heading_confidence
This is the confidence that the desired position was reached.
Definition wedgebug.c:156
struct image_t img_left_int8_cropped
Image obtained from left camera, converted into 8bit gray image.
Definition wedgebug.c:77
uint8_t median_disparity_to_point(struct point_t *Vi, struct image_t *img, struct kernel_C1 *kernel_median)
Definition wedgebug.c:1003
enum navigation_state current_state
Definition wedgebug.c:217
struct FloatVect3 VGOALwned
Declared vector of coordinates of goal in ENU world coordinate system.
Definition wedgebug.c:122
uint16_t median_depth_to_point(struct point_t *Vi, struct image_t *img, struct kernel_C1 *kernel_median)
Definition wedgebug.c:1059
struct FloatVect3 VEDGECOORDINATESwned
Declared vector of coordinates of "best" edge detected in robot coordinate system.
Definition wedgebug.c:129
static struct image_t * copy_right_img_func(struct image_t *img, uint8_t camera_id)
Definition wedgebug.c:545
int SE_dilation_OCV_1
SE size for the closing operation.
Definition wedgebug.c:108
uint8_t is_right_reached_flag
Definition wedgebug.c:237
static struct image_t * copy_left_img_func(struct image_t *img, uint8_t camera_id)
Definition wedgebug.c:535
const char * get_img_type(enum image_type img_type)
Definition wedgebug.c:357
void Vi_to_Vc_depth(struct FloatVect3 *scene_point, int32_t image_point_y, int32_t image_point_x, const float depth, const uint16_t f)
Definition wedgebug.c:755
struct img_size_t img_dims
Definition wedgebug.c:95
int min_disparity
Block size used for the block matching (SBM) function.
Definition wedgebug.c:257
void thresholding_img(struct image_t *img, uint8_t threshold)
Definition wedgebug.c:667
uint8_t is_path_free_flag
Set to 1 if obstacle is detected, 0 otherwise.
Definition wedgebug.c:173
struct kernel_C1 median_kernel
Dimensions of the kernel that detect median disparity in front of drone (for obstacle detection)
Definition wedgebug.c:102
int SE_sobel_OCV
SE size for the erosion operation (see state 3 "WEDGEBUG_START" and state 6 "POSITION_EDGE",...
Definition wedgebug.c:111
float heading_max_right
Definition wedgebug.c:234
float threshold_distance_to_goal_direct
Below this distance (in radians) it is considered that the robot has reached the target angle.
Definition wedgebug.c:147
int16_t position_confidence
This is the confidence that no obstacle was spotted.
Definition wedgebug.c:155
struct FloatVect3 VNEDwenu
Definition wedgebug.c:193
clock_t clock_total_time_previous
Definition wedgebug.c:248
float b
Definition wedgebug.c:202
void show_image_entry(struct image_t *img, int entry_position, const char *img_name)
Definition wedgebug.c:381
float dispfixed_to_disp(const int16_t d)
Definition wedgebug.c:698
struct FloatRMat Rrwned
Definition wedgebug.c:195
float threshold_distance_to_goal
Above this disparity edges are eligible for WedgeBug algorithm (i.e. edges cannot be very far away)
Definition wedgebug.c:145
uint8_t median_disparity_in_front
The maximum angle (in adians) to the left and right of the drone, that edges can be detected in....
Definition wedgebug.c:261
uint8_t is_heading_reached_flag
Set to 1 if no obstacle is detected, 0 otherwise.
Definition wedgebug.c:174
struct FloatVect3 VSTARTwenu
Height of kernel for the median kernel.
Definition wedgebug.c:117
int16_t max_edge_found_micro_confidence
This is the max confidence that a specific heading was reached.
Definition wedgebug.c:165
int SE_erosion_OCV
SE size for the second dilation operation (see state 3 "WEDGEBUG_START" and state 6 "POSITION_EDGE" )
Definition wedgebug.c:110
float threshold_distance_to_goal_manual
Definition wedgebug.c:280
void UYVYs_interlacing_V(struct image_t *YY, struct image_t *left, struct image_t *right)
Definition wedgebug.c:555
int SE_dilation_OCV_2
SE size for the first dilation operation.
Definition wedgebug.c:109
uint8_t previous_mode
Variable that saves previous state the state machine was in, for some memory.
Definition wedgebug.c:254
uint8_t allow_state_change_MOVE_TO_GOAL
Definition wedgebug.c:271
uint8_t is_total_timer_on_flag
Definition wedgebug.c:279
void background_processes_16bit(uint8_t save_images_flag)
Definition wedgebug.c:1481
struct crop_t edge_search_area
Principal point of cropped camera images.
Definition wedgebug.c:187
uint8_t getMedian(uint8_t *a, uint32_t n)
Definition wedgebug.c:469
uint8_t depth_to_disp(const float depth, const float b, const uint16_t f)
Definition wedgebug.c:712
int16_t max_free_path_confidence
This is the max confidence that an obstacle was spotted.
Definition wedgebug.c:162
void Vw_to_Vc(struct FloatVect3 *Vc, struct FloatVect3 *Vw, struct FloatRMat *Rrw, struct FloatVect3 *VRw, struct FloatRMat *Rcr, struct FloatVect3 *VCr, const uint8_t verbose)
Definition wedgebug.c:886
struct image_t img_middle_int8_cropped
Image holding depth values (cm) obtained from the disparity image.
Definition wedgebug.c:81
int16_t free_path_confidence
This is the confidence that an obstacle was spotted.
Definition wedgebug.c:154
float disp_to_depth(const uint8_t d, const float b, const uint16_t f)
Definition wedgebug.c:691
void post_disparity_crop_rect(struct crop_t *_img_cropped_info, struct img_size_t *_original_img_dims, const int disp_n, const int block_size)
Definition wedgebug.c:401
int block_size_disparities
Number of disparity levels (0-this number)
Definition wedgebug.c:256
float float_vect3_norm_two_points(struct FloatVect3 *V1, struct FloatVect3 *V2)
Definition wedgebug.c:970
struct image_t img_depth_int16_cropped
Image obtained after simple block matching.
Definition wedgebug.c:80
struct crop_t img_cropped_info
Definition wedgebug.c:92
control_mode_state
Definition wedgebug.c:220
@ DIRECT_CONTROL
Definition wedgebug.c:221
@ AUTONOMOUS_NAV
Definition wedgebug.c:222
@ AUTONOMOUS_GUIDED
Definition wedgebug.c:223
uint16_t K_median_w
SE size for the Sobel operation, to detect edges.
Definition wedgebug.c:112
void Vb_to_Va(struct FloatVect3 *Va, struct FloatVect3 *Vb, struct FloatRMat *Rba, struct FloatVect3 *VOa)
Definition wedgebug.c:872
uint8_t allow_state_change_WEDGEBUG_START
Definition wedgebug.c:273
double counter_state[NUMBER_OF_STATES]
Definition wedgebug.c:244
int16_t edge_found_macro_confidence
This is the confidence that the desired heading is reached.
Definition wedgebug.c:159
struct FloatVect3 VGOALc
Declared vector of coordinates of goal in robot coordinate system.
Definition wedgebug.c:124
int16_t max_no_edge_found_confidence
This is the max confidence that edges (macro-see above were found.
Definition wedgebug.c:167
uint16_t median_depth_in_front
Variable to hold the median disparity in front of the drone. Needed to see if obstacle is there.
Definition wedgebug.c:262
struct FloatVect3 VEDGECOORDINATESc
Declared vector of coordinates of goal in camera coordinate system.
Definition wedgebug.c:125
uint8_t is_edge_found_macro_flag
Set to 1 if heading is reached, 0 otherwise.
Definition wedgebug.c:175
void show_image_data(struct image_t *img)
Definition wedgebug.c:370
struct image_t img_middle_int16_cropped
Definition wedgebug.c:85
void kernel_free(struct kernel_C1 *kernel)
Definition wedgebug.c:455
#define WEDGEBUG_CAMERA_RIGHT_FPS
Definition wedgebug.c:62
uint8_t is_start_reached_flag
This is the max confidence that no edges were found.
Definition wedgebug.c:170
float heading_initial
Definition wedgebug.c:232
struct FloatVect3 VEDGECOORDINATESwenu
Declared vector of coordinates of "best" edge detected in NED world coordinate system.
Definition wedgebug.c:131
uint8_t is_setpoint_reached_flag
Definition wedgebug.c:171
uint8_t number_of_states
Definition wedgebug.c:264
int N_disparities
Variable that saves previous mode to control the drone, for some memory.
Definition wedgebug.c:255
int16_t max_obstacle_confidence
This is the confidence that no edge was found.
Definition wedgebug.c:161
float heading
Definition wedgebug.c:258
float distance_traveled
Variable to hold the median depth in front of the drone. Needed to see if obstacle is there.
Definition wedgebug.c:263
void Vi_to_Vc(struct FloatVect3 *scene_point, int32_t image_point_y, int32_t image_point_x, const uint8_t d, const float b, const uint16_t f)
Definition wedgebug.c:722
struct FloatVect3 VGOALr
Declared vector of coordinates of goal in NED world coordinate system.
Definition wedgebug.c:123
clock_t clock_FSM
Definition wedgebug.c:250
uint8_t allow_state_change_MOVE_TO_EDGE
Definition wedgebug.c:274
uint8_t threshold_median_disparity
Edges with a magnitude above this value are detected. Above this value, edges are given the value 127...
Definition wedgebug.c:143
uint8_t threshold_disparity_of_edges
Above this median disparity, an obstacle is considered to block the way (i.e. the blocking obstacle n...
Definition wedgebug.c:144
struct FloatVect3 VEDGECOORDINATESr
Declared vector of coordinates of "best" edge detected in camera coordinate system.
Definition wedgebug.c:127
int SE_opening_OCV
Definition wedgebug.c:106
void kernel_create(struct kernel_C1 *kernel, uint16_t width, uint16_t height, enum image_type type)
Definition wedgebug.c:427
struct FloatVect3 VRwned
Definition wedgebug.c:196
int32_t indx1d_c(const int32_t y, const int32_t x, const uint16_t img_height, const uint16_t img_width)
Definition wedgebug.c:836
uint8_t is_state_changed_flag
Set to 1 if no edge was identified, 0 otherwise.
Definition wedgebug.c:178
int16_t obstacle_confidence
Below this depth (m) edges are eligible for the WedgeBug algorithm (i.e. edges cannot be very far awa...
Definition wedgebug.c:153
uint8_t are_setpoint_and_angle_reached(struct FloatVect3 *VGOAL, struct FloatVect3 *VCURRENTPOSITION, float threshold_setpoint, float target_angle, float current_angle, float threshold_angle)
Definition wedgebug.c:1374
int32_t indx1d_b(const int32_t y, const int32_t x, const struct img_size_t *img_dims)
Definition wedgebug.c:822
uint8_t find_best_edge_coordinates(struct FloatVect3 *VEDGECOORDINATESc, struct FloatVect3 *VTARGETc, struct image_t *img_edges, struct image_t *img_disparity, struct crop_t *edge_search_area, uint8_t threshold, int16_t max_confidence)
Definition wedgebug.c:1119
int16_t no_edge_found_confidence
This is the confidence that an edge was found - outside of the find_best_edge_coordinates function.
Definition wedgebug.c:160
double counter_cycles
Definition wedgebug.c:245
struct ES_angles initial_heading
Definition wedgebug.c:239
struct kernel_C1 median_kernel16bit
Definition wedgebug.c:103
struct image_t img_left_int8
Image obtained from right camera (UYVY format)
Definition wedgebug.c:76
struct image_t img_edges_int8_cropped
Definition wedgebug.c:82
void principal_points(struct point_t *c_output, const struct point_t *c_old_input, struct crop_t *img_cropped_info)
Definition wedgebug.c:683
float heading_towards_waypoint(uint8_t wp)
Definition wedgebug.c:978
uint8_t initiated
Definition wedgebug.c:235
int threshold_edge_magnitude
Declared vector of coordinates of previous "best" edge detected in NED world coordinate system.
Definition wedgebug.c:142
void UYVYs_interlacing_H(struct image_t *merged, struct image_t *left, struct image_t *right)
Definition wedgebug.c:594
clock_t clock_total_time_current
Definition wedgebug.c:247
uint8_t is_left_reached_flag
Definition wedgebug.c:236
struct FloatVect3 VCr
Definition wedgebug.c:199
struct image_t img_right
Image obtained from left camera (UYVY format)
Definition wedgebug.c:75
struct image_t img_right_int8
Definition wedgebug.c:78
struct FloatRMat Rwnedwenu
This structure holds information about the window in which edges are searched in.
Definition wedgebug.c:192
void Vc_to_Vw(struct FloatVect3 *Vw, struct FloatVect3 *Vc, struct FloatRMat *Rrw, struct FloatVect3 *VRw, struct FloatRMat *Rcr, struct FloatVect3 *VCr, const uint8_t verbose)
Definition wedgebug.c:942
uint16_t threshold_depth_of_edges
Below this median disparity, an obstacle is considered to block the way (i.e. the blocking obstacle n...
Definition wedgebug.c:150
struct image_t img_disparity_int16_cropped
Image obtained from the external sobel edge detection function = sobel_OCV.
Definition wedgebug.c:84
struct FloatVect3 VSTARTwned
Declared vector of coordinates of start position in ENU world coordinate system.
Definition wedgebug.c:119
struct FloatVect3 VDISTANCEPOSITIONwned
Declared vector of coordinates of "best" edge detected in ENU world coordinate system.
Definition wedgebug.c:133
uint8_t save_images_flag
Set to 1 if control mode of drone is changed, 0 otherwise.
Definition wedgebug.c:180
uint16_t w
Definition wedgebug.h:44
uint32_t buf_size
Size of values of weight buffer and values buffer.
Definition wedgebug.h:46
enum image_type type
Definition wedgebug.h:43
void * buf_values
Kernel value buffer. These are the values underneath the kernel.
Definition wedgebug.h:48
uint16_t h
Kernel height.
Definition wedgebug.h:45
int SBM_OCV(struct image_t *img_disp, const struct image_t *img_left, const struct image_t *img_right, const int ndisparities, const int SADWindowSize, const bool cropped)
int closing_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
int sobel_OCV(struct image_t *img_input, const struct image_t *img_output, const int kernel_size, const int thr)
int save_image_gray(struct image_t *img, char *myString)
int dilation_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
int erosion_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
int opening_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
int save_image_HM(struct image_t *img, char *myString, int const heatmap)