52 #include "generated/flight_plan.h"
61 #ifndef WEDGEBUG_CAMERA_RIGHT_FPS
62 #define WEDGEBUG_CAMERA_RIGHT_FPS 0 //< Default FPS (zero means run at camera fps)
64 #ifndef WEDGEBUG_CAMERA_LEFT_FPS
65 #define WEDGEBUG_CAMERA_LEFT_FPS 0 //< Default FPS (zero means run at camera fps)
202 float b = WEDGEBUG_CAMERA_BASELINE /
289 const char *img_name);
294 const int block_size);
340 float threshold_setpoint,
float target_angle,
float current_angle,
float threshold_angle);
372 default:
return "Image type not found";
381 printf(
"Image-Width: %d\n", img->
w);
382 printf(
"Image-Height: %d\n", img->
h);
383 printf(
"Image-Buf_Size: %d\n", img->
buf_size);
384 printf(
"Image-Buf_Memory_Occupied: %lu\n",
sizeof(img->
buf));
391 printf(
"Pixel %d value - %s: %d\n", entry_position, img_name, ((
uint8_t *)img->
buf)[entry_position]);
397 printf(
"[[%f, %f, %f]\n", R->
m[0], R->
m[1], R->
m[2]);
398 printf(
" [%f, %f, %f]\n", R->
m[3], R->
m[4], R->
m[5]);
399 printf(
" [%f, %f, %f]]\n", R->
m[6], R->
m[7], R->
m[8]);
410 const int block_size)
413 uint16_t block_size_black = block_size / 2;
414 uint16_t left_black = disp_n + block_size_black;
417 img_cropped_info->
y = block_size_black;
418 img_cropped_info->
h = original_img_dims->
h - block_size_black;
419 img_cropped_info->
h = img_cropped_info->
h - img_cropped_info->
y;
421 img_cropped_info->
x = left_black - 1;
422 img_cropped_info->
w = original_img_dims->
w - block_size_black;
423 img_cropped_info->
w = img_cropped_info->
w - img_cropped_info->
x;
483 for (i = 0; i < n; ++i) {
486 for (i = n - 1; i > 0; --i) {
487 for (j = 0; j < i; ++j) {
488 if (dpSorted[j] > dpSorted[j + 1]) {
490 dpSorted[j] = dpSorted[j + 1];
491 dpSorted[j + 1] = dTemp;
499 dMedian = (dpSorted[n / 2] + dpSorted[(n / 2) - 1]) / 2.0;
501 dMedian = dpSorted[n / 2];
514 for (i = 0; i < n; ++i) {
517 for (i = n - 1; i > 0; --i) {
518 for (j = 0; j < i; ++j) {
519 if (dpSorted[j] > dpSorted[j + 1]) {
521 dpSorted[j] = dpSorted[j + 1];
522 dpSorted[j + 1] = dTemp;
530 dMedian = (dpSorted[n / 2] + dpSorted[(n / 2) - 1]) / 2.0;
532 dMedian = dpSorted[n / 2];
566 if (left->
w != right->
w || left->
h != right->
h) {
567 printf(
"The dimensions of the left and right image to not match!");
570 if ((merged->
w * merged->
h) != (2 * right->
w) * right->
w) {
571 printf(
"The dimensions of the empty image template for merger are not sufficient to merge gray left and right pixel values.");
585 for (
uint32_t i = 0; i < loop_length; i++) {
605 if (left->
w != right->
w || left->
h != right->
h) {
606 printf(
"The dimensions of the left and right image to not match!");
609 if ((merged->
w * merged->
h) != (2 * right->
w) * right->
w) {
610 printf(
"The dimensions of the empty image template for merger are not sufficient to merge gray left and right pixel values.");
648 if (*intensity > max) {
655 for (
uint32_t i = 0; i < (img->
w * img->
h); i++) {
658 if (*intensity > max) {
660 printf(
"%d\n", *intensity);
661 printf(
"location = %d\n", i);
666 printf(
"ERROR: function does not support image type %d. Breaking out of function.", img->
type);
680 if (*intensity >= threshold) {
693 c_output->
y = c_old_input->
y - img_cropped_info->
y;
694 c_output->
x = c_old_input->
x - img_cropped_info->
x;
722 return round(b * f / depth);
742 scene_point->
z = 0.0001;
751 scene_point->
y = image_point_y * scene_point -> z /
f;
754 scene_point->
x = image_point_x * scene_point -> z /
f;
767 scene_point->
z = depth;
770 scene_point->
y = image_point_y * scene_point -> z /
f;
773 scene_point->
x = image_point_x * scene_point -> z /
f;
794 scene_point->
z = 0.0001;
803 scene_point->
y = image_point_y * scene_point -> z /
f;
806 scene_point->
x = image_point_x * scene_point -> z /
f;
817 if (x >= (img->
w) || x < 0) {
818 printf(
"Error: index x=%d is out of bounds for axis 0 with size %d. Returning -1\n", x, img->
w);
820 }
else if (y >= (img->
h) || y < 0) {
821 printf(
"Error: index y=%d is out of bounds for axis 0 with size %d. Returning -1\n", y, img->
h);
824 return x + img->
w * y;
832 if (x >= (img_dims->
w) || x < 0) {
833 printf(
"Error: index %d is out of bounds for axis 0 with size %d. Returning -1\n", x, img_dims->
w);
835 }
else if (y >= (img_dims->
h) || y < 0) {
836 printf(
"Error: index %d is out of bounds for axis 0 with size %d. Returning -1\n", y, img_dims->
h);
839 return x + img_dims->
w * y;
846 if (x >= (img_width) || x < 0) {
847 printf(
"Error: index x=%d is out of bounds for axis 0 with size %d. Returning -1\n", x, img_width);
849 }
else if (y >= (img_height) || y < 0) {
850 printf(
"Error: index y=%d is out of bounds for axis 1 with size %d. Returning -1\n", y, img_height);
853 return x + img_width * y;
869 Va_temp.
x = Va->
x - VOa->
x;
870 Va_temp.
y = Va->
y - VOa->
y;
871 Va_temp.
z = Va->
z - VOa->
z;
887 Va->
x = Va->
x + VOa->
x;
888 Va->
y = Va->
y + VOa->
y;
889 Va->
z = Va->
z + VOa->
z;
903 printf(
"%f, %f, %f,\n", Rrw->
m[0], Rrw->
m[1], Rrw->
m[2]);
904 printf(
"%f, %f, %f,\n", Rrw->
m[3], Rrw->
m[4], Rrw->
m[5]);
905 printf(
"%f, %f, %f\n\n", Rrw->
m[6], Rrw->
m[7], Rrw->
m[8]);
908 printf(
"VRw (drone location)\n");
909 printf(
" %f\n %f\n %f\n\n", VRw->
x, VRw->
y, VRw->
z);
913 printf(
" %f\n %f\n %f\n\n", Vw->
x, Vw->
y, Vw->
z);
924 printf(
" %f\n %f\n %f\n\n", Vr.
x, Vr.
y, Vr.
z);
928 printf(
"%f, %f, %f,\n", Rcr->
m[0], Rcr->
m[1], Rcr->
m[2]);
929 printf(
"%f, %f, %f,\n", Rcr->
m[3], Rcr->
m[4], Rcr->
m[5]);
930 printf(
"%f, %f, %f\n\n", Rcr->
m[6], Rcr->
m[7], Rcr->
m[8]);
933 printf(
"VCa (camera location)\n");
934 printf(
" %f\n %f\n %f\n\n", VCr->
x, VCr->
y, VCr->
z);
944 printf(
" %f\n %f\n %f\n\n", Vc->
x, Vc->
y, Vc->
z);
961 printf(
"Vr - back calculated\n"); \
962 printf(
" %f\n %f\n %f\n\n", Vr.
x, Vr.
y, Vr.
z);
972 printf(
"Vw - back calculated\n");
973 printf(
" %f\n %f\n %f\n\n", Vw->
x, Vw->
y, Vw->
z);
993 angle = atan2f(difference.
x, difference.
y);
1005 angle = atan2f(difference.
y, difference.
x);
1014 uint8_t VSTARTi_y = Vi->
y - (kernel_median->
h / 2);
1015 uint8_t VSTARTi_x = Vi->
x - (kernel_median->
w / 2);
1016 uint8_t VSTOPi_y = Vi->
y + (kernel_median->
h / 2);
1017 uint8_t VSTOPi_x = Vi->
x + (kernel_median->
w / 2);
1021 if (VSTOPi_y > img->
h) {
1022 VSTOPi_y = (img->
h - 1);
1024 if (VSTOPi_x > img->
w) {
1025 VSTOPi_x = (img->
w - 1);
1043 for (
uint8_t Vi_y = VSTARTi_y; Vi_y < (VSTOPi_y + 1); Vi_y++) {
1044 for (
uint8_t Vi_x = VSTARTi_x; Vi_x < (VSTOPi_x + 1); Vi_x++) {
1046 Vk_y = Vi_y - VSTARTi_y;
1047 Vk_x = Vi_x - VSTARTi_x;
1050 index_img =
indx1d_a(Vi_y, Vi_x, img);
1070 uint16_t VSTARTi_y = Vi->
y - (kernel_median->
h / 2);
1071 uint16_t VSTARTi_x = Vi->
x - (kernel_median->
w / 2);
1072 uint16_t VSTOPi_y = Vi->
y + (kernel_median->
h / 2);
1073 uint16_t VSTOPi_x = Vi->
x + (kernel_median->
w / 2);
1077 if (VSTOPi_y > img->
h) {
1078 VSTOPi_y = (img->
h - 1);
1080 if (VSTOPi_x > img->
w) {
1081 VSTOPi_x = (img->
w - 1);
1099 for (
uint16_t Vi_y = VSTARTi_y; Vi_y < (VSTOPi_y + 1); Vi_y++) {
1100 for (
uint16_t Vi_x = VSTARTi_x; Vi_x < (VSTOPi_x + 1); Vi_x++) {
1102 Vk_y = Vi_y - VSTARTi_y;
1103 Vk_x = Vi_x - VSTARTi_x;
1106 index_img =
indx1d_a(Vi_y, Vi_x, img);
1132 struct image_t *img_disparity,
1144 float f_distance_edge_to_goal;
1145 float f_distance_robot_to_edge;
1154 VROBOTCENTERc.
x = 0.0; VROBOTCENTERc.
y = 0.0; VROBOTCENTERc.
z = 0.0;
1155 VCLOSESTEDGEi.
x = 0; VCLOSESTEDGEi.
y = 0;
1162 for (
uint16_t y = edge_search_area->
y;
y < (edge_search_area->
y + edge_search_area->
h);
1164 for (
uint16_t x = edge_search_area->
x;
x < (edge_search_area->
x + edge_search_area->
w);
x++) {
1166 edge_value = ((
uint8_t *) img_edges->
buf)[indx];
1167 disparity = ((
uint8_t *) img_disparity->
buf)[indx];
1173 if ((edge_value != 0) && (disparity > threshold)) {
1176 Bound(confidence, 0, max_confidence);
1182 Vi_to_Vc(&VEDGEc, y_from_c, x_from_c, disparity,
b,
f);
1191 if ((f_distance_robot_to_edge + f_distance_edge_to_goal) < distance) {
1193 VEDGECOORDINATESc->
x = VEDGEc.
x;
1194 VEDGECOORDINATESc->
y = VEDGEc.
y;
1195 VEDGECOORDINATESc->
z = VEDGEc.
z;
1197 disparity_best = disparity;
1199 distance = (f_distance_robot_to_edge + f_distance_edge_to_goal);
1201 VCLOSESTEDGEi.
y =
y;
1202 VCLOSESTEDGEi.
x =
x;
1223 if (confidence == max_confidence) {
1225 printf(
"Viable closest edge found: [%d, %d] (disparity = %d)\n", VCLOSESTEDGEi.
y, VCLOSESTEDGEi.
x, disparity_best);
1227 printf(
"At distance: %f\n", distance);
1231 printf(
"No viable edge found\n");
1258 float f_distance_edge_to_goal;
1259 float f_distance_robot_to_edge;
1268 VROBOTCENTERc.
x = 0.0; VROBOTCENTERc.
y = 0.0; VROBOTCENTERc.
z = 0.0;
1269 VCLOSESTEDGEi.
x = 0; VCLOSESTEDGEi.
y = 0;
1276 for (
uint16_t y = edge_search_area->
y;
y < (edge_search_area->
y + edge_search_area->
h);
1278 for (
uint16_t x = edge_search_area->
x;
x < (edge_search_area->
x + edge_search_area->
w);
x++) {
1280 edge_value = ((
uint8_t *) img_edges->
buf)[indx];
1287 if ((edge_value != 0) && (depth < threshold)) {
1290 Bound(confidence, 0, max_confidence);
1307 if ((f_distance_robot_to_edge + f_distance_edge_to_goal) < distance) {
1309 VEDGECOORDINATESc->
x = VEDGEc.
x;
1310 VEDGECOORDINATESc->
y = VEDGEc.
y;
1311 VEDGECOORDINATESc->
z = VEDGEc.
z;
1315 distance = (f_distance_robot_to_edge + f_distance_edge_to_goal);
1317 VCLOSESTEDGEi.
y =
y;
1318 VCLOSESTEDGEi.
x =
x;
1339 if (confidence == max_confidence) {
1341 printf(
"Viable closest edge found: [%d, %d] (depth = %f)\n", VCLOSESTEDGEi.
y, VCLOSESTEDGEi.
x, (depth_best / 100.00));
1343 printf(
"At distance: %f\n", distance);
1347 printf(
"No viable edge found\n");
1366 float target_current_diff;
1367 target_current_diff = target_angle - current_angle;
1371 return sqrt(pow(target_current_diff, 2));
1384 float target_angle,
float current_angle,
float threshold_angle)
1396 float disparity = 1;
1402 for (
int32_t i = 0; i < (img_input->
h * img_input->
w); i++) {
1409 printf(
"ERROR: function does not support image type %d. Breaking out of function.", img_input->
type);
1419 disparity = disparity * 100;
1427 ((
uint16_t *)img16bit_output->
buf)[i] = round(disparity);
1616 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,
1868 default: {printf(
"Unsupported control mode");}
break;
1974 printf(
"median_depth_in_front = %f\n", depth);
1975 printf(
"depth to goal = %f\n",
VGOALc.
z);
1979 printf(
"Obstacle is in front of goal\n");
1981 printf(
"The path to the goal is free!\n");
2015 printf(
"Goal is reached\n");
2042 printf(
"Total time to reach goal = %f\n", ((
double)
clock_total_time) / CLOCKS_PER_SEC);
2050 printf(
"default = %d\n", 0);
2055 printf(
"Time elapsed since start = %f\n", (((
double)clock()) - ((
double)
clock_total_time_current)) / CLOCKS_PER_SEC);
2094 printf(
"Metric 1 was started\n");
2140 printf(
"Goal is reached\n");
2169 printf(
"Object detected!!!!!!!!\n");
2179 printf(
"median_depth_in_front = %f\n", depth);
2180 printf(
"depth to goal = %f\n",
VGOALc.
z);
2186 printf(
"Increasing confidence\n");
2218 printf(
"Total time to reach goal = %f\n", ((
double)
clock_total_time) / CLOCKS_PER_SEC);
2220 printf(
"Average runtime of background processes = %f\n",
2241 printf(
"Holding point is reached\n");
2294 printf(
"Edge not found!!!!!!!!!!!!!!!!!\n");
2363 printf(
"Heading is reached\n");
2391 printf(
"Edge is reached\n");
2422 printf(
"Position and Heading are reached\n");
2442 printf(
"Heading is reached\n");
2472 printf(
"Object detected!!!!!!!!\n");
2481 printf(
"Path is free\n");
2490 printf(
"Depth to goal = %f\n",
VGOALc.
z);
2569 float distance_total = f_distance_robot_to_edge + f_distance_edge_to_goal;
2570 printf(
"distance_total =%f\n", distance_total);
2600 printf(
"Left heading is/was reached\n");
2626 printf(
"Right heading is/was reached\n");
2650 printf(
"Edge has been found\n");
2653 printf(
"Minimum has been encountered\n");
2677 printf(
"default = %d\n", 0);
2689 printf(
"Time elapsed since start = %f\n", ((
double)
clock_total_time) / CLOCKS_PER_SEC);
2703 printf(
"median_depth_in_front = %f\n", depth);
2704 printf(
"depth to goal = %f\n",
VGOALc.
z);
2708 printf(
"Obstacle is in front of goal\n");
2710 printf(
"The path to the goal is free!\n");
2750 {printf(
"Unsupported control mode");}
break;
bool guidance_v_set_guided_z(float z)
Set z setpoint in GUIDED mode.
#define FLOAT_ANGLE_NORMALIZE(_a)
uint16_t getMedian16bit(uint16_t *a, uint32_t n)
uint32_t buf_size
Size of values of weight buffer and values buffer.
struct FloatVect3 VGOALwenu
Declared vector of coordinates of start position in NED world coordinate system.
int closing_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
float float_vect3_norm_two_points(struct FloatVect3 *V1, struct FloatVect3 *V2)
struct image_t img_left_int8_cropped
Image obtained from left camera, converted into 8bit gray image.
uint32_t buf_size
The buffer size.
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
float disp_to_depth(const uint8_t d, const float b, const uint16_t f)
struct image_t img_depth_int16_cropped
Image obtained after simple block matching.
struct img_size_t img_cropped_dims
Dimensions of images captured by the camera (left and right have same dimension)
int SE_closing_OCV
SE size for the opening operation.
struct FloatVect3 VEDGECOORDINATESr
Declared vector of coordinates of "best" edge detected in camera coordinate system.
int save_image_gray(struct image_t *img, char *myString)
void UYVYs_interlacing_V(struct image_t *YY, struct image_t *left, struct image_t *right)
uint16_t h
height of the cropped area
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
int16_t free_path_confidence
This is the confidence that an obstacle was spotted.
uint8_t is_left_reached_flag
int heat_map_type
Variable to hold distance from robot to edge to goal (used in EDGE_SCAN (9) state) ...
void set_state(uint8_t state, uint8_t change_allowed)
struct img_size_t kernel_median_dims
Dimension of image after it has been cropped to remove pixel error due to block matching limitations...
int block_size_disparities
Number of disparity levels (0-this number)
void UYVYs_interlacing_H(struct image_t *merged, struct image_t *left, struct image_t *right)
double time_state[NUMBER_OF_STATES]
uint8_t threshold_median_disparity
Edges with a magnitude above this value are detected. Above this value, edges are given the value 127...
#define VECT3_DIFF(_c, _a, _b)
#define WEDGEBUG_CAMERA_LEFT_FPS
uint8_t is_total_timer_on_flag
uint16_t K_median_h
Width of kernel for the median kernel.
enum navigation_state current_state
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
bool guidance_h_set_guided_pos(float x, float y)
Set horizontal position setpoint in GUIDED mode.
struct FloatRMat Rwnedwenu
This structure holds information about the window in which edges are searched in. ...
double counter_state[NUMBER_OF_STATES]
#define VECT2_DIFF(_c, _a, _b)
Autopilot guided mode interface.
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.
int16_t max_edge_found_micro_confidence
This is the max confidence that a specific heading was reached.
int16_t max_heading_confidence
This is the max confidence that a specific position was reached.
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)
int16_t max_no_edge_found_confidence
This is the max confidence that edges (macro-see above were found.
struct image_t img_right
Image obtained from left camera (UYVY format)
uint16_t y
Start position y (vertical)
int N_disparities
Variable that saves previous mode to control the drone, for some memory.
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)
uint16_t x
Start position x (horizontal)
int16_t obstacle_confidence
Below this depth (m) edges are eligible for the WedgeBug algorithm (i.e. edges cannot be very far awa...
uint32_t x
The x coordinate of the point.
float threshold_distance_to_angle
Below this distance (in meters) it is considered that the robot has reached the goal.
uint8_t save_images_flag
Set to 1 if control mode of drone is changed, 0 otherwise.
int16_t max_free_path_confidence
This is the max confidence that an obstacle was spotted.
int16_t max_obstacle_confidence
This is the confidence that no edge was found.
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
void background_processes(uint8_t save_images_flag)
uint8_t is_edge_found_macro_flag
Set to 1 if heading is reached, 0 otherwise.
const char * get_img_type(enum image_type img_type)
struct FloatVect3 VGOALr
Declared vector of coordinates of goal in NED world coordinate system.
uint8_t is_state_changed_flag
Set to 1 if no edge was identified, 0 otherwise.
uint8_t is_right_reached_flag
void guidance_h_hover_enter(void)
clock_t clock_total_time_previous
float threshold_distance_to_goal_manual
struct video_listener * cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps)
uint8_t previous_mode
Variable that saves previous state the state machine was in, for some memory.
uint8_t is_path_free_flag
Set to 1 if obstacle is detected, 0 otherwise.
struct image_t img_left_int8
Image obtained from right camera (UYVY format)
struct crop_t edge_search_area
Principal point of cropped camera images.
float float_norm_two_angles(float target_angle, float current_angle)
uint8_t is_heading_reached_flag
Set to 1 if no obstacle is detected, 0 otherwise.
void kernel_free(struct kernel_C1 *kernel)
uint8_t is_edge_found_micro_flag
Set to 1 if best edge (according to macro confidence) was found, 0 otherwise.
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
uint32_t maximum_intensity(struct image_t *img)
uint16_t w
Width of the cropped area.
navigation_state
Camera focal length, in pixels (i.e. distance between camera.
static float float_vect3_norm(struct FloatVect3 *v)
uint8_t median_disparity_to_point(struct point_t *Vi, struct image_t *img, struct kernel_C1 *kernel_median)
Paparazzi floating point math for geodetic calculations.
int SE_dilation_OCV_2
SE size for the first dilation operation.
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)
uint8_t is_start_reached_flag
This is the max confidence that no edges were found.
int dilation_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
int16_t edge_found_macro_confidence
This is the confidence that the desired heading is reached.
uint8_t allow_state_change_WEDGEBUG_START
struct img_size_t img_dims
void show_image_entry(struct image_t *img, int entry_position, const char *img_name)
Image helper functions like resizing, color filter, converters...
int16_t no_edge_found_confidence
This is the confidence that an edge was found - outside of the find_best_edge_coordinates function...
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
struct point_t c_img_cropped
Principal point of normal camera images.
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
struct FloatVect3 VHOLDINGPOINTwned
Declared a vector to hold the current position, which is needed for calculating the distance traveled...
uint8_t is_heading_reached(float target_angle, float current_angle, float threshold)
enum control_mode_state current_mode
uint8_t getMedian(uint8_t *a, uint32_t n)
Paparazzi floating point algebra.
An image to hold disparity image data from openCV (int16 per pixel)
Paparazzi generic algebra macros.
uint8_t allow_state_change_POSITION_GOAL
uint16_t threshold_median_depth
Below this distance (in meters) it is considered that the robot has reached the goal, in DIRECT_CONTROL mode.
void thresholding_img(struct image_t *img, uint8_t threshold)
void disp_to_depth_img(struct image_t *img8bit_input, struct image_t *img16bit_output)
struct crop_t img_cropped_info
float threshold_distance_to_goal
Above this disparity edges are eligible for WedgeBug algorithm (i.e. edges cannot be very far away) ...
uint8_t is_no_edge_found_flag
Set to 1 if best edge (according to micro confidence) was found, 0 otherwise.
void principal_points(struct point_t *c_output, const struct point_t *c_old_input, struct crop_t *img_cropped_info)
Computer vision framework for onboard processing.
struct image_t img_right_int8
void * buf_weights
Kernel weight buffer.
void * buf
Image buffer (depending on the image_type)
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)
float heading_towards_waypoint(uint8_t wp)
uint8_t threshold_disparity_of_edges
Above this median disparity, an obstacle is considered to block the way (i.e. the blocking obstacle n...
void Vb_to_Va(struct FloatVect3 *Va, struct FloatVect3 *Vb, struct FloatRMat *Rba, struct FloatVect3 *VOa)
int32_t indx1d_c(const int32_t y, const int32_t x, const uint16_t img_height, const uint16_t img_width)
uint8_t is_obstacle_detected_flag
Set to 1 if setpoint is reached, 0 otherwise.
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup) ...
int threshold_edge_magnitude
Declared vector of coordinates of previous "best" edge detected in NED world coordinate system...
struct image_t img_disparity_int8_cropped
Image obtained from right camera, converted into 8bit gray image.
void show_image_data(struct image_t *img)
#define WEDGEBUG_CAMERA_RIGHT_FPS
float disp_to_depth_16bit(const int16_t d, const float b, const uint16_t f)
clock_t clock_total_time_current
uint32_t y
The y coordinate of the point.
void image_to_grayscale(struct image_t *input, struct image_t *output)
Convert an image to grayscale.
void * buf_values
Kernel value buffer. These are the values underneath the kernel.
struct ES_angles initial_heading
int16_t max_edge_found_macro_confidence
This is the max confidence that edges (micro-see above) were found.
clock_t clock_background_processes
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)
Core autopilot interface common to all firmwares.
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)
uint8_t allow_state_change_EDGE_SCAN
struct FloatVect3 VGOALc
Declared vector of coordinates of goal in robot coordinate system.
static struct image_t * copy_left_img_func(struct image_t *img)
uint8_t allow_state_change_MOVE_TO_GOAL
uint8_t depth_to_disp(const float depth, const float b, const uint16_t f)
API to get/set the generic vehicle states.
int32_t indx1d_a(const int32_t y, const int32_t x, const struct image_t *img)
float max_edge_search_angle
Variable for storing the heading of the drone (psi in radians)
uint8_t is_setpoint_reached(struct FloatVect3 *VGOAL, struct FloatVect3 *VCURRENTPOSITION, float threshold)
int SE_sobel_OCV
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)
int32_t indx1d_b(const int32_t y, const int32_t x, const struct img_size_t *img_dims)
void kernel_create(struct kernel_C1 *kernel, uint16_t width, uint16_t height, enum image_type type)
struct FloatVect3 VSTARTwenu
Height of kernel for the median kernel.
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...
int min_disparity
Block size used for the block matching (SBM) function.
int16_t position_confidence
This is the confidence that no obstacle was spotted.
struct FloatVect3 VEDGECOORDINATESc
Declared vector of coordinates of goal in camera coordinate system.
struct FloatVect3 VEDGECOORDINATESwned
Declared vector of coordinates of "best" edge detected in robot coordinate system.
uint8_t is_setpoint_reached_flag
static void float_vect_copy(float *a, const float *b, const int n)
a = b
uint8_t allow_state_change_POSITION_EDGE
struct image_t img_disparity_int16_cropped
Image obtained from the external sobel edge detection function = sobel_OCV.
float heading_towards_setpoint_WNED(struct FloatVect3 *VSETPOINTwned)
uint16_t median_depth_to_point(struct point_t *Vi, struct image_t *img, struct kernel_C1 *kernel_median)
UYVY format (uint16 per pixel)
static struct image_t * copy_right_img_func(struct image_t *img)
struct FloatVect3 VNEDwenu
void show_rotation_matrix(struct FloatRMat *R)
float dispfixed_to_disp(const int16_t d)
int save_image_HM(struct image_t *img, char *myString, int const heatmap)
float threshold_distance_to_goal_direct
Below this distance (in radians) it is considered that the robot has reached the target angle...
struct FloatVect3 VEDGECOORDINATESwenu
Declared vector of coordinates of "best" edge detected in NED world coordinate system.
float distance_robot_edge_goal
Grayscale image with only the Y part (uint8 per pixel)
struct kernel_C1 median_kernel16bit
bool guidance_h_set_guided_heading(float heading)
Set heading setpoint in GUIDED mode.
#define AP_MODE_ATTITUDE_DIRECT
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)
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)
int16_t max_position_confidence
This is the max confidence that an obstacle was not spotted.
uint16_t median_depth_in_front
Variable to hold the median disparity in front of the drone. Needed to see if obstacle is there...
void Va_to_Vb(struct FloatVect3 *Vb, struct FloatVect3 *Va, struct FloatRMat *Rba, struct FloatVect3 *VOa)
An image gradient (int16 per pixel)
int sobel_OCV(struct image_t *img_input, const struct image_t *img_output, const int kernel_size, const int thr)
int opening_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
uint16_t K_median_w
SE size for the Sobel operation, to detect edges.
uint16_t threshold_depth_of_edges
Below this median disparity, an obstacle is considered to block the way (i.e. the blocking obstacle n...
struct kernel_C1 median_kernel
Dimensions of the kernel that detect median disparity in front of drone (for obstacle detection) ...
void background_processes_16bit(uint8_t save_images_flag)
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
struct FloatVect3 VGOALwned
Declared vector of coordinates of goal in ENU world coordinate system.
#define AP_MODE_ATTITUDE_Z_HOLD
struct FloatVect3 VSTARTwned
Declared vector of coordinates of start position in ENU world coordinate system.
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)
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)
int erosion_OCV(struct image_t *img_input, const struct image_t *img_output, const int SE_size, const int iteration)
uint8_t is_mode_changed_flag
Set to 1 if state was changed, 0 otherwise.
int SE_erosion_OCV
SE size for the second dilation operation (see state 3 "WEDGEBUG_START" and state 6 "POSITION_EDGE" )...
bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
Set position and heading setpoints in GUIDED mode.
struct FloatVect3 VDISTANCEPOSITIONwned
Declared vector of coordinates of "best" edge detected in ENU world coordinate system.
struct image_t img_edges_int8_cropped
struct FloatVect3 VPBESTEDGECOORDINATESwned
Declared a vector to hold the position of a holding point (offten used to make sure drone stays still...
float distance_traveled
Variable to hold the median depth in front of the drone. Needed to see if obstacle is there...
int SE_dilation_OCV_1
SE size for the closing operation.
uint8_t allow_state_change_MOVE_TO_EDGE
int16_t heading_confidence
This is the confidence that the desired position was reached.
uint8_t autopilot_get_mode(void)
get autopilot mode
An JPEG encoded image (not per pixel encoded)
enum image_type type
The image type.
struct image_t img_middle_int16_cropped
struct image_t img_middle_int8_cropped
Image holding depth values (cm) obtained from the disparity image.