diff --git a/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo.xml b/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo.xml index 09ebfbfd685..ade6c19142d 100644 --- a/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo.xml +++ b/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo.xml @@ -1,5 +1,5 @@ + -
#include "modules/datalink/datalink.h" @@ -19,11 +19,10 @@ - - - - - + + + + diff --git a/conf/userconf/tudelft/course_control_panel.xml b/conf/userconf/tudelft/course_control_panel.xml index 41219314702..b112fd557d3 100644 --- a/conf/userconf/tudelft/course_control_panel.xml +++ b/conf/userconf/tudelft/course_control_panel.xml @@ -97,7 +97,6 @@ - diff --git a/sw/airborne/modules/computer_vision/lib/vision/image.c b/sw/airborne/modules/computer_vision/lib/vision/image.c index e0170f834ae..e8b5fa0624b 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/image.c +++ b/sw/airborne/modules/computer_vision/lib/vision/image.c @@ -999,3 +999,184 @@ void image_draw_line_color(struct image_t *img, struct point_t *from, struct poi } } } + + +/** + * Kernel multiplication for a 3 x 3 kernel. + * Pixels outside the edges are taken to be 0. + * @param[in] source The source values + * @param[in] kernel The kernel values + * @param total The kernel total, set to 1 if the result should not be normalised + * @param setting Variable is used to indicate which pixels the multiplication uses at a specific edges. + * @param YUV A bool that adds an extra pixel offset if the YUV format is used. + */ +uint8_t ker_mul_3x3(uint8_t const *source, int_fast8_t const *kernel, uint8_t total, uint8_t setting, int width, int YUV) { + int value; + int offset = 1 + YUV; + + switch (setting) { + default: { // No edge + value = *(source - offset - width) * kernel[0] + *(source - width) * kernel[1] + *(source + offset - width) * kernel[2] + + *(source - offset) * kernel[3] + *source * kernel[4] + *(source + offset) * kernel[5] + + *(source - offset + width) * kernel[6] + *(source + width) * kernel[7] + *(source + offset + width) * kernel[8]; + break; } + case 1: { // Upper Edge + value = *(source - offset) * kernel[3] + *source * kernel[4] + *(source + offset) * kernel[5] + + *(source - offset + width) * kernel[6] + *(source + width) * kernel[7] + *(source + offset + width) * kernel[8]; + break; } + case 2: { // Lower Edge + value = *(source - offset - width) * kernel[0] + *(source - width) * kernel[1] + *(source + offset - width) * kernel[2] + + *(source - offset) * kernel[3] + *source * kernel[4] + *(source + offset) * kernel[5]; + break; } + case 3: { // Left Edge + value = *(source - width) * kernel[1] + *(source + offset - width) * kernel[2] + + *source * kernel[4] + *(source + offset) * kernel[5] + + *(source + width) * kernel[7] + *(source + offset + width) * kernel[8]; + break; } + case 4: { // Right Edge + value = *(source - offset - width) * kernel[0] + *(source - width) * kernel[1] + + *(source - offset) * kernel[3] + *source * kernel[4] + + *(source - offset + width) * kernel[6] + *(source + width) * kernel[7]; + break; } + case 5: {// Top Left Corner + value = *source * kernel[4] + *(source + offset) * kernel[5] + + *(source + width) * kernel[7] + *(source + offset + width) * kernel[8]; + break; } + case 6: {// Top Right Corner + value = *(source - offset) * kernel[3] + *source * kernel[4] + + *(source - offset + width) * kernel[6] + *(source + width) * kernel[7]; + break; } + case 7: {// Bottom Left Corner + value = *(source - width) * kernel[1] + *(source + offset - width) * kernel[2] + + *source * kernel[4] + *(source + offset) * kernel[5]; + break; } + case 8: {// Bottom Right Corner + value = *(source - offset - width) * kernel[0] + *(source - width) * kernel[1] + + *(source - offset) * kernel[3] + *source * kernel[4]; + break; } + } + + return (uint8_t) ((int) abs(value) / total); +} + +/** + * Image convolution for a 3x3 kernel + * @param[in] input Input image + * @param[in,out] output Output image + * @param[in] kernel The kernel values +*/ +void image_convolution_3x3(struct image_t *input, struct image_t *output, int_fast8_t const *kernel, uint8_t kernel_total) { + // Copy buffer pointers + uint8_t *source = input->buf; + uint8_t *dest = output->buf; + + // Copy the creation timestamp (stays the same) + output->ts = input->ts; + output->eulers = input->eulers; + output->pprz_ts = input->pprz_ts; + + int height = output->h; + int width = output->w; + + if (output->type == IMAGE_YUV422) { + // Skip The first U/V value + *dest++ = 127; + source++; + + // Top Left Corner + *dest++ = ker_mul_3x3(source, kernel, kernel_total, 5, 2 * width, 1); + source += 2; + *dest++ = 127; + + // Upper Edge + for (int x = 1; x < width - 1; x++) { + *dest++ = ker_mul_3x3(source, kernel, kernel_total, 1, 2 * width, 1); + source += 2; + *dest++ = 127; + } + + // Top Right Corner + *dest++ = ker_mul_3x3(source, kernel, kernel_total, 6, 2 * width, 1); + source += 2; + *dest++ = 127; + + for (int y = 1; y < height - 1; y++) { + // Left Edge + *dest++ = ker_mul_3x3(source, kernel, kernel_total, 3, 2 * width, 1); + source += 2; + *dest++ = 127; + + // Middle + for (int x = 1; x < width - 1; x++) { + *dest++ = ker_mul_3x3(source, kernel, kernel_total, 0, 2 * width, 1); + source += 2; + *dest++ = 127; + } + + // Right Edge + *dest++ = ker_mul_3x3(source, kernel, kernel_total, 4, 2 * width, 1); + source += 2; + *dest++ = 127; + } + + // Bottom Left Corner + *dest++ = ker_mul_3x3(source, kernel, kernel_total, 7, 2 * width, 1); + source += 2; + *dest++ = 127; + + // Bottom Edge + for (int x = 1; x < width - 1; x++) { + *dest++ = ker_mul_3x3(source, kernel, kernel_total, 2, 2 * width, 1); + source += 2; + *dest++ = 127; + } + + // Bottom Right Corner + *dest = ker_mul_3x3(source, kernel, kernel_total, 8, 2 * width, 1); + } else { + if (output->type == IMAGE_GRAYSCALE) { + // Top Left Corner + *dest++ = ker_mul_3x3(source, kernel, kernel_total, 5, width, 0); + source++; + + // Upper Edge + for (int x = 1; x < width - 1; x++) { + *dest++ = ker_mul_3x3(source, kernel, kernel_total, 1, width, 0); + source++; + } + + // Top Right Corner + *dest++ = ker_mul_3x3(source, kernel, kernel_total, 6, width, 0); + source++; + + for (int y = 1; y < height - 1; y++) { + // Left Edge + *dest++ = ker_mul_3x3(source, kernel, kernel_total, 3, width, 0); + source++; + + // Middle + for (int x = 1; x < width - 1; x++) { + *dest++ = ker_mul_3x3(source, kernel, kernel_total, 0, width, 0); + source++; + } + + // Right Edge + *dest++ = ker_mul_3x3(source, kernel, kernel_total, 4, width, 0); + source++; + } + + // Bottom Left Corner + *dest++ = ker_mul_3x3(source, kernel, kernel_total, 7, width, 0); + source++; + + // Bottom Edge + for (int x = 1; x < width - 1; x++) { + *dest++ = ker_mul_3x3(source, kernel, kernel_total, 2, width, 0); + source++; + } + + // Bottom Right Corner + *dest = ker_mul_3x3(source, kernel, kernel_total, 8, width, 0); + } + } +} diff --git a/sw/airborne/modules/computer_vision/lib/vision/image.h b/sw/airborne/modules/computer_vision/lib/vision/image.h index b7b4ffee6d5..cd3eaabdafe 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/image.h +++ b/sw/airborne/modules/computer_vision/lib/vision/image.h @@ -126,5 +126,7 @@ void image_draw_line_color(struct image_t *img, struct point_t *from, struct poi void pyramid_next_level(struct image_t *input, struct image_t *output, uint8_t border_size); void pyramid_build(struct image_t *input, struct image_t *output_array, uint8_t pyr_level, uint16_t border_size); void image_gradient_pixel(struct image_t *img, struct point_t *loc, int method, int *dx, int *dy); +uint8_t ker_mul_3x3(uint8_t const *source, int_fast8_t const *kernel, uint8_t total, uint8_t setting, int width, int YUV); +void image_convolution_3x3(struct image_t *input, struct image_t *output, int_fast8_t const *kernel, uint8_t kernel_total); #endif diff --git a/sw/tools/opti_dist/dist.py b/sw/tools/opti_dist/dist.py index 3bb6bd7bb80..13251234444 100755 --- a/sw/tools/opti_dist/dist.py +++ b/sw/tools/opti_dist/dist.py @@ -5,6 +5,7 @@ import math import argparse import matplotlib.pyplot as plt +import matplotlib.patches as mpatches # if PAPARAZZI_HOME not set, then assume the tree containing this # file is a reasonable substitute @@ -13,6 +14,15 @@ from NatNetClient import NatNetClient +recording = False + +def on_press(event): + global recording + if event.key == 'r': + recording = not recording + if event.key == 'q': + plt.close() + # This is a callback function that gets connected to the NatNet client. It is called once per rigid body per frame def receiveRigidBodyFrame(rigidBodyList, stamp): @@ -39,6 +49,12 @@ def main(args): fig = plt.figure() plt.axis([-6, 6, -6, 6]) + # add key press event + fig.canvas.mpl_connect('key_press_event', on_press) + + # title + plt.title('Press r to start/stop recording \n press q to quit') + # This will create a new NatNet client streamingClient = NatNetClient( server=args.server, @@ -46,7 +62,7 @@ def main(args): commandPort=args.commandPort, dataPort=args.dataPort, rigidBodyListListener=receiveRigidBodyFrame, - version=(2,9,0,0)) + version=(3,0,0,0)) # Start up the streaming client now that the callbacks are set up. # This will run perpetually, and operate on a separate thread. streamingClient.run() @@ -55,7 +71,7 @@ def main(args): print('Start tracking') if args.outputfile: file = open(args.outputfile, 'w') - file.write('timestamp, x, y, z\n') + file.write('time, distance, x, y, z, recording \n') old_z = pos_z old_x = pos_x @@ -64,22 +80,33 @@ def main(args): pre_time = time.time() while plt.fignum_exists(fig.number): - if args.outputfile: - data = '{}, {}, {}, {}\n'.format(int((time.time() - start_time) * 1000), pos_x, pos_y, pos_z) - file.write(data) h = math.hypot(pos_z - old_z, pos_x - old_x) - if h > 0.20: - distance += h + + if h > 0.10: + if recording: + distance += h old_z = pos_z old_x = pos_x - if time.time() - pre_time > 0.5: - print("distance:%3.4f m; time_step:%d" % (distance, int((time.time() - start_time) * 2))) + if time.time() - pre_time > .1: + current_time = time.time() - start_time + print(f'distance: {distance}; time: {current_time:.2f}; recording: {recording}') pre_time = time.time() - plt.plot(pos_z, pos_x, 'ro') - plt.draw() + + if args.outputfile: + # data = '{}, {}, {}, {}, {}, {} \n'.format(time.time() - start_time, distance, pos_x, pos_y, pos_z, recording) + data = f'{current_time}, {distance}, {pos_x}, {pos_y}, {pos_z}, {recording} \n' + file.write(data) + + if recording: + plt.plot(pos_z, pos_x, 'ro') + plt.title('Press r to start/stop recording \n press q to quit \n RECORDING') + else: + plt.plot(pos_z, pos_x, 'bo') + plt.title('Press r to start/stop recording \n press q to quit \n NOT RECORDING') + plt.pause(0.001) - time.sleep(0.01) + # time.sleep(0.01) streamingClient.stop() if args.outputfile: