52#include "generated/flight_plan.h"
61#ifndef WEDGEBUG_CAMERA_RIGHT_FPS
62#define WEDGEBUG_CAMERA_RIGHT_FPS 0
64#ifndef WEDGEBUG_CAMERA_LEFT_FPS
65#define WEDGEBUG_CAMERA_LEFT_FPS 0
364 default:
return "Image type not found";
375 printf(
"Image-Buf_Size: %d\n",
img->buf_size);
376 printf(
"Image-Buf_Memory_Occupied: %lu\n",
sizeof(
img->buf));
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]);
402 const int block_size)
475 for (i = 0; i < n; ++i) {
478 for (i = n - 1; i > 0; --i) {
479 for (
j = 0;
j < i; ++
j) {
506 for (i = 0; i < n; ++i) {
509 for (i = n - 1; i > 0; --i) {
510 for (
j = 0;
j < i; ++
j) {
559 printf(
"The dimensions of the left and right image to not match!");
563 printf(
"The dimensions of the empty image template for merger are not sufficient to merge gray left and right pixel values.");
598 printf(
"The dimensions of the left and right image to not match!");
602 printf(
"The dimensions of the empty image template for merger are not sufficient to merge gray left and right pixel values.");
653 printf(
"location = %d\n", i);
658 printf(
"ERROR: function does not support image type %d. Breaking out of function.",
img->type);
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);
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);
816 return x +
img->w * y;
825 printf(
"Error: index %d is out of bounds for axis 0 with size %d. Returning -1\n", x,
_img_dims->w);
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);
839 printf(
"Error: index x=%d is out of bounds for axis 0 with size %d. Returning -1\n", x,
img_width);
842 printf(
"Error: index y=%d is out of bounds for axis 1 with size %d. Returning -1\n", y,
img_height);
900 printf(
"VRw (drone location)\n");
925 printf(
"VCa (camera location)\n");
953 printf(
"Vr - back calculated\n");
\
964 printf(
"Vw - back calculated\n");
1219 printf(
"At distance: %f\n", distance);
1223 printf(
"No viable edge found\n");
1335 printf(
"At distance: %f\n", distance);
1339 printf(
"No viable edge found\n");
1401 printf(
"ERROR: function does not support image type %d. Breaking out of function.",
img_input->type);
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,
1860 default: {
printf(
"Unsupported control mode");}
break;
1971 printf(
"Obstacle is in front of goal\n");
1973 printf(
"The path to the goal is free!\n");
2007 printf(
"Goal is reached\n");
2042 printf(
"default = %d\n", 0);
2086 printf(
"Metric 1 was started\n");
2132 printf(
"Goal is reached\n");
2161 printf(
"Object detected!!!!!!!!\n");
2178 printf(
"Increasing confidence\n");
2212 printf(
"Average runtime of background processes = %f\n",
2233 printf(
"Holding point is reached\n");
2286 printf(
"Edge not found!!!!!!!!!!!!!!!!!\n");
2355 printf(
"Heading is reached\n");
2383 printf(
"Edge is reached\n");
2414 printf(
"Position and Heading are reached\n");
2434 printf(
"Heading is reached\n");
2464 printf(
"Object detected!!!!!!!!\n");
2473 printf(
"Path is free\n");
2592 printf(
"Left heading is/was reached\n");
2618 printf(
"Right heading is/was reached\n");
2642 printf(
"Edge has been found\n");
2645 printf(
"Minimum has been encountered\n");
2669 printf(
"default = %d\n", 0);
2700 printf(
"Obstacle is in front of goal\n");
2702 printf(
"The path to the goal is free!\n");
2742 {
printf(
"Unsupported control mode");}
break;
uint8_t autopilot_get_mode(void)
get autopilot mode
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 WaypointAlt(_wp)
waypoint altitude in m above MSL
struct video_listener * cv_add_to_device(struct video_config_t *device, cv_function func, uint16_t fps, uint8_t id)
Computer vision framework for onboard processing.
#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.
#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).
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
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.
void image_to_grayscale(struct image_t *input, struct image_t *output)
Convert an image to grayscale.
void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type)
Create a new image.
Image helper functions like resizing, color filter, converters...
uint16_t h
height of the cropped area
uint16_t w
Width of the cropped area.
uint32_t x
The x coordinate of the point.
uint16_t x
Start position x (horizontal)
uint16_t y
Start position y (vertical)
uint32_t y
The y coordinate of the point.
enum image_type type
The image type.
@ IMAGE_GRAYSCALE
Grayscale image with only the Y part (uint8 per pixel)
@ IMAGE_YUV422
UYVY format (uint16 per pixel)
@ IMAGE_GRADIENT
An image gradient (int16 per pixel)
@ IMAGE_INT16
An image to hold disparity image data from openCV (int16 per pixel)
@ IMAGE_JPEG
An JPEG encoded image (not per pixel encoded)
Paparazzi generic algebra macros.
Paparazzi floating point algebra.
Paparazzi floating point math for geodetic calculations.
#define AP_MODE_ATTITUDE_DIRECT
#define AP_MODE_ATTITUDE_Z_HOLD
void guidance_h_set_pos(float x, float y)
Set horizontal position setpoint.
void guidance_h_hover_enter(void)
void guidance_h_set_heading(float heading)
Set heading setpoint.
void guidance_v_set_z(float z)
Set z position setpoint.
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)
uint16_t K_median_h
Width of kernel for the median kernel.
struct FloatVect3 VHOLDINGPOINTwned
Declared a vector to hold the current position, which is needed for calculating the distance traveled...
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
void set_state(uint8_t _state, uint8_t change_allowed)
uint16_t threshold_median_depth
Below this distance (in meters) it is considered that the robot has reached the goal,...
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)
float disp_to_depth_16bit(const int16_t d, const float b, const uint16_t f)
uint8_t allow_state_change_POSITION_EDGE
uint8_t is_obstacle_detected_flag
Set to 1 if setpoint is reached, 0 otherwise.
#define WEDGEBUG_CAMERA_LEFT_FPS
uint8_t is_setpoint_reached(struct FloatVect3 *VGOAL, struct FloatVect3 *VCURRENTPOSITION, float threshold)
enum control_mode_state current_mode
uint8_t is_no_edge_found_flag
Set to 1 if best edge (according to micro confidence) was found, 0 otherwise.
uint8_t is_mode_changed_flag
Set to 1 if state was changed, 0 otherwise.
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)
int16_t max_heading_confidence
This is the max confidence that a specific position was reached.
float threshold_distance_to_angle
Below this distance (in meters) it is considered that the robot has reached the goal.
float float_norm_two_angles(float target_angle, float current_angle)
navigation_state
Camera focal length, in pixels (i.e. distance between camera.
uint8_t allow_state_change_EDGE_SCAN
uint8_t is_heading_reached(float target_angle, float current_angle, float threshold)
struct FloatVect3 VGOALwenu
Declared vector of coordinates of start position in NED world coordinate system.
struct point_t c_img_cropped
Principal point of normal camera images.
struct img_size_t kernel_median_dims
Dimension of image after it has been cropped to remove pixel error due to block matching limitations.
void disp_to_depth_img(struct image_t *img8bit_input, struct image_t *img16bit_output)
struct img_size_t img_cropped_dims
Dimensions of images captured by the camera (left and right have same dimension)
int heat_map_type
Variable to hold distance from robot to edge to goal (used in EDGE_SCAN (9) state)
int SE_closing_OCV
SE size for the opening operation.
double time_state[NUMBER_OF_STATES]
struct FloatVect3 VPBESTEDGECOORDINATESwned
Declared a vector to hold the position of a holding point (offten used to make sure drone stays still...
void background_processes(uint8_t save_images_flag)
void show_rotation_matrix(struct FloatRMat *R)
struct image_t img_disparity_int8_cropped
Image obtained from right camera, converted into 8bit gray image.
int16_t max_edge_found_macro_confidence
This is the max confidence that edges (micro-see above) were found.
int32_t indx1d_a(const int32_t y, const int32_t x, const struct image_t *img)
float distance_robot_edge_goal
clock_t clock_background_processes
uint8_t is_edge_found_micro_flag
Set to 1 if best edge (according to macro confidence) was found, 0 otherwise.
uint32_t maximum_intensity(struct image_t *img)
void Va_to_Vb(struct FloatVect3 *Vb, struct FloatVect3 *Va, struct FloatRMat *Rba, struct FloatVect3 *VOa)
float heading_towards_setpoint_WNED(struct FloatVect3 *VSETPOINTwned)
uint8_t allow_state_change_POSITION_GOAL
uint16_t getMedian16bit(uint16_t *a, uint32_t n)
int16_t max_position_confidence
This is the max confidence that an obstacle was not spotted.
int16_t heading_confidence
This is the confidence that the desired position was reached.
struct image_t img_left_int8_cropped
Image obtained from left camera, converted into 8bit gray image.
uint8_t median_disparity_to_point(struct point_t *Vi, struct image_t *img, struct kernel_C1 *kernel_median)
enum navigation_state current_state
struct FloatVect3 VGOALwned
Declared vector of coordinates of goal in ENU world coordinate system.
uint16_t median_depth_to_point(struct point_t *Vi, struct image_t *img, struct kernel_C1 *kernel_median)
struct FloatVect3 VEDGECOORDINATESwned
Declared vector of coordinates of "best" edge detected in robot coordinate system.
static struct image_t * copy_right_img_func(struct image_t *img, uint8_t camera_id)
int SE_dilation_OCV_1
SE size for the closing operation.
uint8_t is_right_reached_flag
static struct image_t * copy_left_img_func(struct image_t *img, uint8_t camera_id)
const char * get_img_type(enum image_type img_type)
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)
struct img_size_t img_dims
int min_disparity
Block size used for the block matching (SBM) function.
void thresholding_img(struct image_t *img, uint8_t threshold)
uint8_t is_path_free_flag
Set to 1 if obstacle is detected, 0 otherwise.
struct kernel_C1 median_kernel
Dimensions of the kernel that detect median disparity in front of drone (for obstacle detection)
int SE_sobel_OCV
SE size for the erosion operation (see state 3 "WEDGEBUG_START" and state 6 "POSITION_EDGE",...
float threshold_distance_to_goal_direct
Below this distance (in radians) it is considered that the robot has reached the target angle.
int16_t position_confidence
This is the confidence that no obstacle was spotted.
struct FloatVect3 VNEDwenu
clock_t clock_total_time_previous
void show_image_entry(struct image_t *img, int entry_position, const char *img_name)
float dispfixed_to_disp(const int16_t d)
float threshold_distance_to_goal
Above this disparity edges are eligible for WedgeBug algorithm (i.e. edges cannot be very far away)
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....
uint8_t is_heading_reached_flag
Set to 1 if no obstacle is detected, 0 otherwise.
struct FloatVect3 VSTARTwenu
Height of kernel for the median kernel.
int16_t max_edge_found_micro_confidence
This is the max confidence that a specific heading was reached.
int SE_erosion_OCV
SE size for the second dilation operation (see state 3 "WEDGEBUG_START" and state 6 "POSITION_EDGE" )
float threshold_distance_to_goal_manual
void UYVYs_interlacing_V(struct image_t *YY, struct image_t *left, struct image_t *right)
int SE_dilation_OCV_2
SE size for the first dilation operation.
uint8_t previous_mode
Variable that saves previous state the state machine was in, for some memory.
uint8_t allow_state_change_MOVE_TO_GOAL
uint8_t is_total_timer_on_flag
void background_processes_16bit(uint8_t save_images_flag)
struct crop_t edge_search_area
Principal point of cropped camera images.
uint8_t getMedian(uint8_t *a, uint32_t n)
uint8_t depth_to_disp(const float depth, const float b, const uint16_t f)
int16_t max_free_path_confidence
This is the max confidence that an obstacle was spotted.
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)
struct image_t img_middle_int8_cropped
Image holding depth values (cm) obtained from the disparity image.
int16_t free_path_confidence
This is the confidence that an obstacle was spotted.
float disp_to_depth(const uint8_t d, const float b, const uint16_t f)
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)
int block_size_disparities
Number of disparity levels (0-this number)
float float_vect3_norm_two_points(struct FloatVect3 *V1, struct FloatVect3 *V2)
struct image_t img_depth_int16_cropped
Image obtained after simple block matching.
struct crop_t img_cropped_info
uint16_t K_median_w
SE size for the Sobel operation, to detect edges.
void Vb_to_Va(struct FloatVect3 *Va, struct FloatVect3 *Vb, struct FloatRMat *Rba, struct FloatVect3 *VOa)
uint8_t allow_state_change_WEDGEBUG_START
double counter_state[NUMBER_OF_STATES]
int16_t edge_found_macro_confidence
This is the confidence that the desired heading is reached.
struct FloatVect3 VGOALc
Declared vector of coordinates of goal in robot coordinate system.
int16_t max_no_edge_found_confidence
This is the max confidence that edges (macro-see above were found.
uint16_t median_depth_in_front
Variable to hold the median disparity in front of the drone. Needed to see if obstacle is there.
struct FloatVect3 VEDGECOORDINATESc
Declared vector of coordinates of goal in camera coordinate system.
uint8_t is_edge_found_macro_flag
Set to 1 if heading is reached, 0 otherwise.
void show_image_data(struct image_t *img)
struct image_t img_middle_int16_cropped
void kernel_free(struct kernel_C1 *kernel)
#define WEDGEBUG_CAMERA_RIGHT_FPS
uint8_t is_start_reached_flag
This is the max confidence that no edges were found.
struct FloatVect3 VEDGECOORDINATESwenu
Declared vector of coordinates of "best" edge detected in NED world coordinate system.
uint8_t is_setpoint_reached_flag
int N_disparities
Variable that saves previous mode to control the drone, for some memory.
int16_t max_obstacle_confidence
This is the confidence that no edge was found.
float distance_traveled
Variable to hold the median depth in front of the drone. Needed to see if obstacle is there.
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)
struct FloatVect3 VGOALr
Declared vector of coordinates of goal in NED world coordinate system.
uint8_t allow_state_change_MOVE_TO_EDGE
uint8_t threshold_median_disparity
Edges with a magnitude above this value are detected. Above this value, edges are given the value 127...
uint8_t threshold_disparity_of_edges
Above this median disparity, an obstacle is considered to block the way (i.e. the blocking obstacle n...
struct FloatVect3 VEDGECOORDINATESr
Declared vector of coordinates of "best" edge detected in camera coordinate system.
void kernel_create(struct kernel_C1 *kernel, uint16_t width, uint16_t height, enum image_type type)
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_state_changed_flag
Set to 1 if no edge was identified, 0 otherwise.
int16_t obstacle_confidence
Below this depth (m) edges are eligible for the WedgeBug algorithm (i.e. edges cannot be very far awa...
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)
int32_t indx1d_b(const int32_t y, const int32_t x, const struct img_size_t *img_dims)
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)
int16_t no_edge_found_confidence
This is the confidence that an edge was found - outside of the find_best_edge_coordinates function.
struct ES_angles initial_heading
struct kernel_C1 median_kernel16bit
struct image_t img_left_int8
Image obtained from right camera (UYVY format)
struct image_t img_edges_int8_cropped
void principal_points(struct point_t *c_output, const struct point_t *c_old_input, struct crop_t *img_cropped_info)
float heading_towards_waypoint(uint8_t wp)
int threshold_edge_magnitude
Declared vector of coordinates of previous "best" edge detected in NED world coordinate system.
void UYVYs_interlacing_H(struct image_t *merged, struct image_t *left, struct image_t *right)
clock_t clock_total_time_current
uint8_t is_left_reached_flag
struct image_t img_right
Image obtained from left camera (UYVY format)
struct image_t img_right_int8
struct FloatRMat Rwnedwenu
This structure holds information about the window in which edges are searched in.
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)
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 image_t img_disparity_int16_cropped
Image obtained from the external sobel edge detection function = sobel_OCV.
struct FloatVect3 VSTARTwned
Declared vector of coordinates of start position in ENU world coordinate system.
struct FloatVect3 VDISTANCEPOSITIONwned
Declared vector of coordinates of "best" edge detected in ENU world coordinate system.
uint8_t save_images_flag
Set to 1 if control mode of drone is changed, 0 otherwise.
uint32_t buf_size
Size of values of weight buffer and values buffer.
void * buf_values
Kernel value buffer. These are the values underneath the kernel.
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)