TUD course update 2024 (#3258)

* removed the -o flag because it caussed natnet errors

* smaller obstacle zone for first week testing

* Updated distance measurement script

* 3x3 convolution addition (#97)

Added a convolution algorithm for a general 3x3 kernel

---------

Co-authored-by: robinferede <robinferede@tudelft.nl>
Co-authored-by: yamacbirol <y.birol@student.tudelft.nl>
Co-authored-by: dvanwolffelaar <50547974+pigparty6@users.noreply.github.com>

conf/flight_plans/tudelft/course_orangeavoid_cyberzoo.xml

@@ -1,5 +1,5 @@
+<?xml version="1.0"?>
 <!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
-
 <flight_plan alt="1.0" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="60" name="Bebop avoid orange TU Delft Cyberzoo" security_height="0.4">
   <header>
     #include "modules/datalink/datalink.h"
@@ -19,11 +19,10 @@
     <waypoint lat="51.9906440" lon="4.3767060" name="_CZ2"/>
     <waypoint lat="51.9906860" lon="4.3768080" name="_CZ3"/>
     <waypoint lat="51.9906238" lon="4.3768729" name="_CZ4"/>
-
-    <waypoint lat="51.9905860" lon="4.3767712" name="_OZ1"/>
-    <waypoint lat="51.9906430" lon="4.3767110" name="_OZ2"/>
-    <waypoint lat="51.9906830" lon="4.3768048" name="_OZ3"/>
-    <waypoint lat="51.9906239" lon="4.3768684" name="_OZ4"/>
+    <waypoint lat="51.9906006" lon="4.3767764" name="_OZ1"/>
+    <waypoint lat="51.9906405" lon="4.3767316" name="_OZ2"/>
+    <waypoint lat="51.9906687" lon="4.3768025" name="_OZ3"/>
+    <waypoint lat="51.9906273" lon="4.3768438" name="_OZ4"/>
   </waypoints>
   <sectors>
     <sector color="red" name="CyberZoo">

conf/userconf/tudelft/course_control_panel.xml

@@ -97,7 +97,6 @@
       </program>
       <program name="NatNet3">
         <arg flag="-ac 9999" constant="@AC_ID"/>
-        <arg flag="-o"/>
         <arg flag="-xa" constant="57"/>
         <arg flag="-an" constant="far"/>
         <arg flag="-le" constant="far"/>

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);
+        }
+    }
+}

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

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,14 +49,20 @@ 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,
         multicast=args.multicast,
         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: