• DepthAI-v2
  • Custom IMU Filter Running on Camera Board

Hi!

In our use-case, we have strong transnational acceleration forces, sometimes for a few minutes (boats). The direct output from the IMU do not perform well ("too much momentum"). I wanted to ask two questions:

  1. Can you point me to resources that have worked on this issue using the Luxonis camera?
  2. We've done experiments with custom filters on the host using acc and gyro - they look promising. How would we implement these on the camera board? Would that work with synchronization nodes and sending them out via HTTP server? Would I have to run them in a custom Script node?

Any insight would help, thanks!

    BenjaminKiefer The direct output from the IMU do not perform well ("too much momentum").

    What do you mean too much momentum? The accelerometer should support ±8g for acceleration.

    Thanks,
    Jaka

      jakaskerl

      I can't tell you what the exact cause for it is, but whenever we experience somewhat strong acceleration forces, it can't cope with that. Just one example using luxonis IMU to project the horizon line from world space to image space using pitch+roll:

      If you'd like, I can share more videos. But, it's kind of expected as the IMU is tuned for application like step counting or other smartphone-based application (refer to their manual, we have the 9-axis IMU).

      Hence, the question 🙂

      Thanks!

        BenjaminKiefer
        Which outputs are you using? How do you compute the horizon tracking line angle? I would expect two scenarios:

        • Latency issues (IMU queues get blocked and cause delay)
        • Wrong reference point is used for calculation

        Thanks,
        Jaka

        I'd also be interested in the calculation of this line. Are you just using accelero info?

        We're using ROTATION_VECTOR. No, we're using the sync node and everything looks fine in-office:

        Essentially, we are projecting two "horizon world space points" onto image space. Green is Luxonis' ROTATION_VECTOR, red is "our" custom Madgwick after sending the raw acc, gyro to host, similar for purple complimentary filter.

        If you're interested in the code, check draw_horizon_line.py

        import depthai as dai
        import csv
        import json
        import time
        import copy
        from pathlib import Path
        import cv2
        from queue import Queue
        import numpy as np
        from scipy.spatial.transform import Rotation as R
        from scipy.signal import butter, filtfilt
        import matplotlib.pyplot as plt
        from datetime import timedelta
        from threading import Event, Thread

        class MadgwickFilter():
        def init(self, beta=0.1):
        self.beta = beta
        self.q = np.array([1.0, 0.0, 0.0, 0.0]) # Initial quaternion (w, x, y, z)

        def update(self, gyro, accel, beta, dt):
            self.beta = beta
            q = self.q
        
            # Convert gyroscope degrees to radians
            gx, gy, gz = gyro
            ax, ay, az = accel
        
            # Normalize accelerometer measurement
            norm_accel = np.sqrt(ax**2 + ay**2 + az**2)
            ax, ay, az = ax / norm_accel, ay / norm_accel, az / norm_accel
        
            # Quaternion components
            q1, q2, q3, q4 = q
            _2q1 = 2 * q1
            _2q2 = 2 * q2
            _2q3 = 2 * q3
            _2q4 = 2 * q4
            _4q1 = 4 * q1
            _4q2 = 4 * q2
            _4q3 = 4 * q3
            _8q2 = 8 * q2
            _8q3 = 8 * q3
            q1q1 = q1 * q1
            q1q2 = q1 * q2
            q1q3 = q1 * q3
            q1q4 = q1 * q4
            q2q2 = q2 * q2
            q2q3 = q2 * q3
            q2q4 = q2 * q4
            q3q3 = q3 * q3
            q3q4 = q3 * q4
            q4q4 = q4 * q4
        
            # Compute the gradient descent step
            f1 = 2 * (q2 * q4 - q1 * q3) - ax
            f2 = 2 * (q1 * q2 + q3 * q4) - ay
            f3 = 1 - 2 * (q2q2 + q3q3) - az
            J = np.array([
                [-_2q3, _2q4, -_2q1, _2q2],
                [_2q2, _2q1, _2q4, _2q3],
                [0, -4 * q2, -4 * q3, 0],
            ])
            step = np.dot(J.T, np.array([f1, f2, f3]))
            step = step / np.linalg.norm(step)
            step *= self.beta
        
            # Update quaternion
            q[0] += (-q[1] * gx - q[2] * gy - q[3] * gz) * (0.5 * dt) + step[0]
            q[1] += (q[0] * gx + q[2] * gz - q[3] * gy) * (0.5 * dt) + step[1]
            q[2] += (q[0] * gy - q[1] * gz + q[3] * gx) * (0.5 * dt) + step[2]
            q[3] += (q[0] * gz + q[1] * gy - q[2] * gx) * (0.5 * dt) + step[3]
        
            # Normalize quaternion
            norm = np.sqrt(np.dot(q, q))
            self.q = q / norm
        
        def get_quaternion(self):
            return self.q

        def stabilize_image(image, roll, pitch):
        (h, w) = image.shape[:2]
        center = (w / 2, h / 2)

        # Compute the rotation matrix for roll
        M_roll = cv2.getRotationMatrix2D(center, -roll, 1.0)  # Roll angle negated for stabilization
        
        # Compute the rotation matrix for pitch by rotating around the horizontal axis
        pitch_radians = -np.radians(pitch)
        M_pitch = np.array([[1, 0, 0],
                            [0, np.cos(pitch_radians), -np.sin(pitch_radians)],
                            [0, np.sin(pitch_radians), np.cos(pitch_radians)]])
        
        # Apply both transformations to the image
        rotated_image_roll = cv2.warpAffine(image, M_roll, (w, h))
        rotated_image = cv2.warpAffine(rotated_image_roll, M_pitch[:2], (w, h))
        
        return rotated_image

        def video_writer_thread(frame_queue, video_path, stop_event):
        fourcc = cv2.VideoWriter_fourcc(*'mp4v')
        out = None

        while not stop_event.is_set() or not frame_queue.empty():
            if not frame_queue.empty():
                frame = frame_queue.get()
                if frame is None:
                    break
                # Initialize video writer once we know the frame size
                if out is None:
                    frame_height, frame_width = frame.shape[:2]
                    out = cv2.VideoWriter(video_path, fourcc, 24, (frame_width, frame_height))
                out.write(frame)
        
        if out:
            out.release()
        
        # viz_out = None
        # while not viz_stop_event.is_set() or not viz_frame_queue.empty():
        #     if not viz_frame_queue.empty():
        #         viz_frame = viz_frame_queue.get()
        #         if viz_frame is None:
        #             break
        #         # Initialize video writer once we know the frame size
        #         if viz_out is None:
        #             frame_height, frame_width = viz_frame.shape[:2]
        #             viz_out = cv2.VideoWriter(viz_video_path, viz_fourcc, 24, (frame_width, frame_height))
        #         viz_out.write(viz_frame)
        
        # if viz_out:
        #     viz_out.release()

        def calculate_frequency(timestamps):
        if len(timestamps) < 2:
        return 0

        # Calculate intervals as timedelta objects
        intervals = np.diff(timestamps)
        
        # # Convert timedelta objects to seconds
        # intervals_in_seconds = np.array([interval for interval in intervals])
        
        # Compute average interval in seconds
        average_interval = np.mean(intervals)
        
        return 1 / average_interval if average_interval > 0 else 0

        def high_pass_filter(data, cutoff, fs, order=5):
        nyquist = 0.5 * fs
        normal_cutoff = cutoff / nyquist
        b, a = butter(order, normal_cutoff, btype='high', analog=False)
        y = filtfilt(b, a, data)
        return y

        def low_pass_filter(data, cutoff, fs, order=5):
        nyquist = 0.5 * fs
        normal_cutoff = cutoff / nyquist
        b, a = butter(order, normal_cutoff, btype='low', analog=False)
        y = filtfilt(b, a, data)
        return y

        def quaternion_to_euler(quaternion):
        r = R.from_quat(quaternion)
        euler_angles = r.as_euler('xyz', degrees=True)
        return euler_angles

        def rotate_image(image, angle):
        (h, w) = image.shape[:2]
        center = (w / 2, h / 2)
        M = cv2.getRotationMatrix2D(center, angle, 1.0)
        rotated_image = cv2.warpAffine(image, M, (w, h))
        return rotated_image

        def shift_for_pitch(image, pitch):
        (h, w) = image.shape[:2]

        # Calculate how much to shift the image based on pitch
        # Scale the pitch to pixel units (adjust scaling factor as needed)
        pitch_shift = int(pitch * h / 90)  # Assume pitch range [-90, 90] maps to full height
        
        # Apply the vertical shift (simulate pointing the camera up/down)
        if pitch_shift > 0:
            shifted_image = image[pitch_shift:, :]  # Crop the top part (pointing downward)
        else:
            shifted_image = image[:h + pitch_shift, :]  # Crop the bottom part (pointing upward)
        
        # Resize the shifted image back to original size to keep the frame consistent
        shifted_image = cv2.resize(shifted_image, (w, h))
        
        return shifted_image

        def format_euler_angles(euler_angles):
        """Format the Euler angles for display."""
        roll, pitch, yaw = euler_angles
        return f"Roll: {roll:.2f} Pitch: {pitch:.2f}"

        def draw_horizon_line(video_frame, roll, pitch, color, f=50, sensor_width=36, sensor_height=20.25):
        height, width, _ = video_frame.shape

        # Convert roll and pitch to radians
        roll_rad = np.radians(roll + 90)
        pitch_rad = np.radians(pitch)
        
        # Calculate rotation matrices
        R_x = np.array([[1, 0, 0],
                        [0, np.cos(roll_rad), -np.sin(roll_rad)],
                        [0, np.sin(roll_rad), np.cos(roll_rad)]])
        
        R_y = np.array([[np.cos(pitch_rad), 0, np.sin(pitch_rad)],
                        [0, 1, 0],
                        [-np.sin(pitch_rad), 0, np.cos(pitch_rad)]])
        
        # Combined rotation matrix
        R = R_y @ R_x
        
        # Camera position and transformation matrix
        t_updated_position = np.array([0, 0, 2])
        T_CW = np.eye(4)
        T_CW[:3, :3] = R
        T_CW[:3, 3] = t_updated_position
        
        # Define two points far ahead in the world space
        P_w_camera_far_point = np.array([0, 5000, 0, 1])
        P_w_camera_second_point = np.array([1000, 5000, 0, 1])
        
        # Transform the points to camera coordinates
        P_c1 = np.linalg.inv(T_CW) @ P_w_camera_far_point
        P_c2 = np.linalg.inv(T_CW) @ P_w_camera_second_point
        
        # Project the points onto the image plane
        x_proj1 = (f * P_c1[0]) / -P_c1[2]
        y_proj1 = (f * P_c1[1]) / -P_c1[2]
        
        x_proj2 = (f * P_c2[0]) / -P_c2[2]
        y_proj2 = (f * P_c2[1]) / -P_c2[2]
        
        # Scaling factors for projection
        alpha_u = 1 / sensor_width
        alpha_v = 1 / sensor_height
        
        # Convert to pixel coordinates
        u1 = int((x_proj1 * alpha_u + 0.5) * width)
        v1 = int((0.5 - y_proj1 * alpha_v) * height)
        
        u2 = int((x_proj2 * alpha_u + 0.5) * width)
        v2 = int((0.5 - y_proj2 * alpha_v) * height)
        
        # Calculate the slope of the line (in image space)
        slope = (v2 - v1) / (u2 - u1)
        
        # Define the line extension to the image boundaries
        u_start = 0
        u_end = width
        v_start = int(v1 + slope * (u_start - u1))
        v_end = int(v1 + slope * (u_end - u1))
        
        # Draw the horizon line
        cv2.line(video_frame, (u_start, v_start), (u_end, v_end), color, 2)  # White horizon line

        plot_angles = True
        record = True
        use_compl = False
        stabilize = True

        Create pipeline

        pipeline = dai.Pipeline()

        Define sources and outputs

        imu = pipeline.create(dai.node.IMU)
        cam_rgb = pipeline.create(dai.node.ColorCamera)

        sync = pipeline.create(dai.node.Sync)
        xout_grp = pipeline.create(dai.node.XLinkOut)
        xout_grp.setStreamName("xout")

        Configure the IMU sensors

        imu.enableIMUSensor(dai.IMUSensor.ACCELEROMETER_RAW, 200)
        imu.enableIMUSensor(dai.IMUSensor.GYROSCOPE_RAW, 200)
        imu.enableIMUSensor(dai.IMUSensor.MAGNETOMETER_RAW, 100) # Add magnetometer raw data
        imu.enableIMUSensor(dai.IMUSensor.ROTATION_VECTOR, 200)

        imu.setBatchReportThreshold(1)
        imu.setMaxBatchReports(10)

        Configure the color camera

        cam_rgb.setBoardSocket(dai.CameraBoardSocket.CAM_D)
        cam_rgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P)
        cam_rgb.setFps(24)

        Apply flipping correction manually via transformation matrix

        #cam_rgb.initialControl.setRotation(180) # This should correctly flip the camera

        sync.setSyncThreshold(timedelta(milliseconds=10))
        sync.setSyncAttempts(-1)

        cam_rgb.video.link(sync.inputs["video"])
        imu.out.link(sync.inputs["imu"])
        sync.out.link(xout_grp.input)

        Connect to the device and start the pipeline

        with dai.Device(pipeline) as device:

        dt = 1 / 24  # 210
        fs = 1 / dt
        # synced queue
        grp_q = device.getOutputQueue(name="xout", maxSize=30, blocking=True)
        
        lp_alpha = 0.5
        hp_alpha = 0.05
        cf_alpha_r = 0.2
        cf_alpha_p = 0.2
        
        md_beta = 0.033
        madgwick = MadgwickFilter(beta=md_beta)
        
        if use_compl:
            unfilt_gr = [0]
            unfilt_gp = [0]
            unfilt_gy = [0]
        
            unfilt_ar = []
            unfilt_ap = []
            unfilt_ay = []
        
        if record:
            print("Recording...")
            print()
        
            # Open CSV file for writing roll, pitch, and yaw estimates
            imu_data_path = "imu_data.json"
            imu_data_dict = {}
        
            imu_data_count = 0
            frame_counter = 0
        
            # Initialize frame queue and video writer thread
            frame_queue = Queue()
            stop_event = Event()
        
            # Start video writer thread
            video_thread = Thread(target=video_writer_thread, args=(frame_queue, "recorded_vid.mp4", stop_event))
            video_thread.start()
        
        if plot_angles:
            frame_rpy = []
        
        accel_timestamps = []
        gyro_timestamps = []
        vid_timestamps = []
        
        axs = []
        ays = []
        azs = []
        acc_mean_size = 50
        
        try:
            while True:
                start_time = time.time()
                if record:
                    imu_packets_list = []
        
                grp_msg = grp_q.get()
                video_frame = grp_msg["video"]
                frame = video_frame.getCvFrame()
        
                frame = rotate_image(frame, 180)
                frame_height, frame_width = frame.shape[:2]
        
                if record:
                    frame_queue.put(copy.deepcopy(frame))
        
                imuData = grp_msg["imu"]
                imuPackets = imuData.packets
                print(f"imu packet length: {len(imuPackets)}")
        
                imu_ts = imuData.getTimestampDevice().total_seconds()
                vid_ts = video_frame.getTimestampDevice().total_seconds()
        
                qs = []
                gyro_x = []
                gyro_y = []
                gyro_z = []
        
                accel_x = []
                accel_y = []
                accel_z = []
        
                for imuPacket in imuPackets:
                    # print(dir(imuPacket))
                    if record:
                        imu_data_count += 1
        
                    acceleroValues = imuPacket.acceleroMeter
                    gyroValues = imuPacket.gyroscope
                    magnetoValues = imuPacket.magneticField  # Magnetometer data
                    rotationVector = imuPacket.rotationVector
                    quaternion = [rotationVector.i, rotationVector.j, rotationVector.k, rotationVector.real]
        
                    accelero_ts = acceleroValues.getTimestampDevice().total_seconds()
                    gyro_ts = gyroValues.getTimestampDevice().total_seconds()
        
                    if record:
                        packet_data = {
                                "video_ts": vid_ts,
                                "imu_ts": imu_ts,
                                "accelero_ts": accelero_ts,
                                "gyro_ts": gyro_ts,
                                "accel_x": acceleroValues.x,
                                "accel_y": acceleroValues.y,
                                "accel_z": acceleroValues.z,
                                "gyro_x": gyroValues.x,
                                "gyro_y": gyroValues.y,
                                "gyro_z": gyroValues.z,
                                "quaternion (i, j, k, real)": quaternion
                        }
                        imu_packets_list.append(packet_data)
        
                    qs.append(quaternion)
        
                    gyro_x.append(gyroValues.x)
                    gyro_y.append(gyroValues.y)
                    gyro_z.append(gyroValues.z)
        
                    accel_x.append(acceleroValues.x)
                    accel_y.append(acceleroValues.y)
                    accel_z.append(acceleroValues.z)
        
                accel_timestamps.append(accelero_ts)
                gyro_timestamps.append(gyro_ts)
                vid_timestamps.append(vid_ts)
        
                gyro_freq = calculate_frequency(gyro_timestamps)
                accel_freq = calculate_frequency(accel_timestamps)
                fps = calculate_frequency(vid_timestamps)
        
                print(f"Gyroscope Frequency: {gyro_freq:.2f} Hz")
                print(f"Accelerometer Frequency: {accel_freq:.2f} Hz")
                print(f"FPS: {fps:.2f}")
                print()
        
                curr_q = np.mean(qs, 0)
                euler_angles = quaternion_to_euler(curr_q)
                euler_angles[0] -= 90
                euler_angles[1] *= -1
                # euler_angles[1] /= 2
        
                lr = euler_angles[0]
                lp = euler_angles[1]
                ly = euler_angles[2]
        
                gx = np.mean(gyro_x, 0)
                gy = np.mean(gyro_y, 0)
                gz = np.mean(gyro_z, 0)
        
                ax = np.mean(accel_x, 0)
                ay = np.mean(accel_y, 0)
                az = np.mean(accel_z, 0)
        
                axs.append(ax)
                ays.append(ay)
                azs.append(az)
        
                if use_compl:
                    ugr = unfilt_gr[-1] + gx * dt
                    ugp = unfilt_gp[-1] + gz * dt
                    ugy = unfilt_gy[-1] + gy * dt
        
                    unfilt_gr.append(ugr)
                    unfilt_gp.append(ugp)
                    unfilt_gy.append(ugy)
        
                    uar = np.arctan2(ay, az)
                    uap = np.arctan2(-ax, np.sqrt(ay ** 2 + az ** 2))
        
                    unfilt_ar.append(uar)
                    unfilt_ap.append(uap)
        
                    if len(unfilt_ar) > 18:
                        accel_roll = low_pass_filter(unfilt_ar, lp_alpha, fs)
                        accel_pitch = low_pass_filter(unfilt_ap, lp_alpha, fs)
        
                        gyro_roll = high_pass_filter(unfilt_gr, hp_alpha, fs)
                        gyro_pitch = high_pass_filter(unfilt_gp, hp_alpha, fs)
                    else:
                        accel_roll = unfilt_ar
                        accel_pitch = unfilt_ap
        
                        gyro_roll = unfilt_gr
                        gyro_pitch = unfilt_gp
                    
                    gr = np.rad2deg(gyro_roll[-1]) # * 10 - 10
                    gp = np.rad2deg(gyro_pitch[-1]) / 2 # * 3
        
                    ar = np.rad2deg(accel_roll[-1]) - 90
                    ap = - np.rad2deg(accel_pitch[-1]) / 2 # - 5
          
                    cr = cf_alpha_r * gr + (1 - cf_alpha_r) * ar
                    cp = cf_alpha_p * gp + (1 - cf_alpha_p) * ap
                    cp += 1
        
                gyro_data = np.array([gx, gy, gz])
                accel_data = np.array([ax, ay, az])
                if accel_freq != 0:
                    dt = 1 / accel_freq
                else:
                    dt = 1 / 24
        
                madgwick.update(gyro=gyro_data, accel=accel_data, beta=md_beta, dt=dt)
                mq = madgwick.get_quaternion()
                new_mq = np.array([mq[1], mq[2], mq[3], mq[0]])
                mr, mp, my = quaternion_to_euler(new_mq)
        
                mr += 90
                mp /= 2
        
                if len(axs) > acc_mean_size:
                    curr_ax = np.mean(axs[-acc_mean_size:])
                    curr_ay = np.mean(ays[-acc_mean_size:])
                    curr_az = np.mean(azs[-acc_mean_size:])
                else:
                    curr_ax = axs[-1]
                    curr_ay = ays[-1]
                    curr_az = azs[-1]
        
                # if curr_ax < -1.5:
                #     mp += 5
                # elif curr_ax > 1.5:
                #     mp -= 5
        
                if plot_angles:
                    frame_rpy.append([lr, lp, ly, mr, mp])
        
                if not stabilize:
                    euler_text = format_euler_angles([lr, lp, 0])
                    madg_text = format_euler_angles([mr, mp, my])
                    if use_compl:
                        compl_text = format_euler_angles([cr, cp, 0])
        
                    viz_frame = cv2.putText(frame, f"Lux: {euler_text}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2, cv2.LINE_AA)
                    viz_frame = cv2.putText(viz_frame, f"Madg: {madg_text}", (10, 70), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2, cv2.LINE_AA)
                    viz_frame = cv2.putText(viz_frame, f"FPS: {fps:.2f}", (1550, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2, cv2.LINE_AA)
                    if use_compl:
                        viz_frame = cv2.putText(viz_frame, f"Compl: {compl_text}", (10, 70), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 255), 2, cv2.LINE_AA)
        
                    viz_frame = cv2.putText(viz_frame, f"Madg Beta: {md_beta:.3f}", (10, 110), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 165, 255), 2, cv2.LINE_AA)
                    viz_frame = cv2.putText(viz_frame, f"Acc: ax: {curr_ax:.2f}, ay: {curr_ay:.2f}, az: {curr_az:.2f}", (10, 150), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2, cv2.LINE_AA)
        
                    draw_horizon_line(viz_frame, lp, lr, color=(0, 255, 0))
                    draw_horizon_line(viz_frame, mp, mr, color=(0, 0, 255))
                    if use_compl:
                        draw_horizon_line(viz_frame, cp, cr, color=(255, 0, 255))
        
                if stabilize:
                    stabilized_frame = rotate_image(frame, -lr)
                    stabilized_frame = shift_for_pitch(stabilized_frame, -lp)
                    cv2.imshow("Stabilized Video", stabilized_frame)
                else:
                    cv2.imshow("frame", viz_frame)
        
                if record:
                    imu_data_dict[frame_counter] = imu_packets_list
                    frame_counter += 1
                    # viz_frame_queue.put(viz_frame)
        
                key = cv2.waitKey(1) & 0xFF
                if key == ord('q'):
                    break
                elif key == ord('w'):
                    md_beta -= 0.1
                elif key == ord('e'):
                    md_beta += 0.1
        
        except KeyboardInterrupt:
            print("Loop terminated!")
        
        if record:
            stop_event.set()
            # viz_stop_event.set()
            video_thread.join()
            with open(imu_data_path, 'w') as json_file:
                json.dump(imu_data_dict, json_file, indent=4)
        cv2.destroyAllWindows()

        if plot_angles:
        fig, axs = plt.subplots(2, 1, figsize=(8, 10)) # 2 rows, 1 column
        frame_rpy = np.array(frame_rpy)

        lr = frame_rpy[:, 0]
        lp = frame_rpy[:, 1]
        ly = frame_rpy[:, 2]
        mr = frame_rpy[:, 3]
        mp = frame_rpy[:, 4]
        
        axs[0].plot(lr, color='g', label='lux roll')
        axs[0].plot(mr, color='r', label='madg roll')
        
        axs[1].plot(lp, color='g', label='lux pitch')
        axs[1].plot(mp, color='r', label='madg pitch')
        
        axs[0].legend()
        axs[1].legend()
        plt.tight_layout()  # Adjusts subplot parameters to give specified padding
        plt.savefig("rp_plot.png")
        plt.show() 

          BenjaminKiefer Thanks for sharing.

          I took a quick look and see a few issues:

          • Using rotation vector: As per documentation of BNO IMU, the rotation vector is is expressed as a quaternion referenced to magnetic north and gravity. However in an accelerating body, this gravity component is somewhat wrong. It is provided by the MEMS accelerometer and is probably selected as a vector of all three (X;Y;Z) accelerometer comopnents. Meaning: when the boat steers there are two forces present: gravity (pointing down) and centrifugal force (pointing outwards). These two combine to create a new reference vector around which the rotation quaternion is referenced. Hence the error.
            EDIT: Haven't tested it, but from the datasheet, seems like AR/VR Stabilized Rotation Vector output should perform better.

          the accelerometer itself can
          create a potential correction in the rotation vector produced (i.e. the estimate of gravity changes). For applications
          (typically augmented or virtual reality applications) where a sudden jump can be disturbing, the output is adjusted
          to prevent these jumps in a manner that takes account of the velocity of the sensor system.

          • Using Madgwick filter: The filter works great! Using a low beta ensures that linear acceleration doesn't affect the orientation measurements too much. This filter should be working relatively good even on an accelerating body, correct? One thing to add: perhaps an adaptive beta could further help in situations like this. Having a linear (or quadratic) function that maps abs(g-acceleration) to beta value. Meaning: when overall acceleration differs from standard gravity (eg. when boat is steering), lower the beta value to further limit the effect of acceleration.
            This filter will however be difficult to implement on-device since script node CPU is not fast enough.

          Another suggestion:
          For good visibility situations: A feature tracker rotation estimation (like this one, but for rotation) could help as well.

          Thanks,
          Jaka

          Ok thanks. Yes, we thought about an adaptive beta value (and ChatGPT also told us some more things), but our experiments are mixed so far.. See the video here:

          https://drive.google.com/drive/folders/13AJnlj48d2deS6ECiF171zxVlK2lXhge?usp=sharing

          We'll try further.

          Meanwhile, it's more important for us to know how to get the data from the board to the host. Synchronized and stable. Is there any way to embed the IMU data directly in the jpeg/h264 frames (as exif, SEI, APP Markers, ..)?

          Otherwise, we have to send them down in a custom manner, and it's already not feeling super stable. We were recommended to not use Xlink but have a standalone mode for more stability. That leaves us with HTTP server or socker server. Both are not that nice for stable video streaming (RTSP or the like would be best).

          What would you recommend in this case?

          Thanks!

            Hi @BenjaminKiefer
            This seem very unreliable despite the filtering.. What exactly do you need the horizon line for?

            Perhaps it would be easier to use a vision model to detect the line. cc @Matija is it possible to only extract the horizon from tersekmatija/eWaSR?

            Thanks,
            Jaka

            For augmented reality we need to know our extrinsics very accurately.

            We are using eWaSR for other purposes already (@Matija is also part of the MaCVi workshop organizers 😉 )

            Where it works good for horizon locking is when the waterline and skyline are somewhat close:

            However, it fails whenever vision conditions are bad (fog, rain, sun reflections, ....) or it hasn't seen training data enough. Also, it's heavy on the hardware compared to a pure IMU-based approach.

            Should I open another issue for the pipeline-related questions?

            Thanks!

            Hi @BenjaminKiefer
            Best to create a separate thread, yes; so we keep only the IMU filtering here.

            BenjaminKiefer Meanwhile, it's more important for us to know how to get the data from the board to the host. Synchronized and stable. Is there any way to embed the IMU data directly in the jpeg/h264 frames (as exif, SEI, APP Markers, ..)?

            Afaik it would be very difficult to embed the data into image itself, but should be possible by appending the data to the mjpeg - like done here: luxonis/depthai-experimentsblob/master/gen2-cumulative-object-counting/standalone/oak.py#L133C13-L133C150

            Thanks,
            Jaka

            Yes, we've done that, but it's not that efficient and supported by standard streaming protocols. E.g., we pipe an MJPEG stream through Deepstream on the host, it's not straight forward to pipe other data alongside with it (at least, I wouldn't know how). We'd need to pipe it separately and then sync again on the frontend, not ideal. Also, we cannot directly feed the custom stream into Deepstream and would have to have a splitter first.

            Thanks anyways!