Take single IMU measurements without constantly looping over the queue.

blank

Hello,

I have a Oak-D Pro mounted to a drone to take pictures for photogrammetric processing.

The camera is mounted on a gimbal and looks straight down, or, at least it's supposed to. Due to imperfect adjustment of the gimbal and sudden movements of the drone, there might be some deviations in the camera angles on some pictures.

Now, I'd like to get the most recent rotations from the camera's IMU as correction values everytime I take a picture. This requires me to get a single IMU value upon request instead of receiving the values in a loop as they are produced. So far, I had no success.

The code I'm using is embedded in my drone operation framework and is hard to phase out on it's own. However, the following portion should give you an idea of what I'm trying to do

	    self.pipeline = dai.Pipeline()

            camRgb = self.pipeline.create(dai.node.ColorCamera)
            camRgb.setBoardSocket(dai.CameraBoardSocket.CAM_A)
            camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_4_K)
            
            xin = self.pipeline.create(dai.node.XLinkIn)
            xin.setStreamName("control")
            xin.out.link(camRgb.inputControl)
            
            # Properties
            videoEnc = self.pipeline.create(dai.node.VideoEncoder)
            videoEnc.setDefaultProfilePreset(1, dai.VideoEncoderProperties.Profile.MJPEG)
            camRgb.still.link(videoEnc.input)
            
            # Linking
            xoutStill = self.pipeline.create(dai.node.XLinkOut)
            xoutStill.setStreamName("still")
            videoEnc.bitstream.link(xoutStill.input)
            
            imu = self.pipeline.create(dai.node.IMU)
            xlinkOutImu = self.pipeline.create(dai.node.XLinkOut)
            xlinkOutImu.setStreamName("imu")
            imu.enableIMUSensor(dai.IMUSensor.ROTATION_VECTOR, 100)
            imu.setBatchReportThreshold(1)
            imu.setMaxBatchReports(10)
            
            self.logger.info('Camera waiting for drone to initialize...')
            while not DroneManager().has_drone_instance():
                #print("Waiting for drone")
                await asyncio.sleep(0.2)
            drone, status_holder  = DroneManager().get_drone_instance()
            while drone.mission_status != Drone.STATUS_INITIALIZED:
                self.logger.debug("Camera waiting for mission to start...")
                await asyncio.sleep(1)
            while not drone.gps_health_ok():
                await asyncio.sleep(0.2)
            self.logger.info("Starting camera...")
                       
            with dai.Device(self.pipeline) as device:
                
                qStill = device.getOutputQueue(name="still", maxSize=30, blocking=True)
                qControl = device.getInputQueue(name="control") 
                imuQueue = device.getOutputQueue(name="imu", maxSize=50, blocking=False)
                
                prior_north_m = float('inf')
                prior_east_m = float('inf')
                
                while True:
                    
                    north_m = drone.telemetry_position_ned.position.north_m
                    east_m = drone.telemetry_position_ned.position.east_m
                    diff_north_m = abs(north_m - prior_north_m) 
                    diff_east_m = abs(east_m - prior_east_m) 
                    diff_m = math.sqrt(diff_north_m**2 + diff_east_m**2)

		    # If drone has traveled x meters since last image... 
                    if diff_m >= self.image_dist_m:
                        
                        # ...take another image
                        ctrl = dai.CameraControl()
                        ctrl.setCaptureStill(True)
                        qControl.send(ctrl)
                        self.logger.debug("Sent 'still' event to the camera!")
                        
                        # When image is ready, query IMU data
                        if qStill.has():
                            imuData = imuQueue.tryGet()
                            imuPackets = imuData.packets
                            rVvalues = imuPackets[0].rotationVector
                            print(rVvalues.i, rVvalues.j, rVvalues.k, rVvalues.real)

As you can see at the bottom of the code, a still image is taken every x meters that the drone travels. Once the image is ready, I try to retrieve the last (or any) data packet the IMU produced. However, imuQueue.tryGet() yields None. If I change this to a blocking call (imuQueue.get()), it simply blocks forever on that line.

I realize I'm absolutely not using this function as intended, but I must be possible to just get a value occassionally and discard the rest, isn't it. I just don't know how.

I might be able to build a query loop for the IMU queue in a new thread. However, that's another thread demanding resources and the drone's computer is already quite at it's limits.

I hope It's clear what I'm trying to achieve. If not, feel free to ask.

Best regards.

jakaskerl

Hi blank
Ensure first that you can read the IMU data as intended. Then make sure to set the IMU queue to non blocking so it doesn't saturate.

ctrl = dai.CameraControl()
ctrl.setCaptureStill(True)
qControl.send(ctrl)
self.logger.debug("Sent 'still' event to the camera!")

# When image is ready
if qStill.has():
    retries = 5
    imuData = None
    while retries > 0 and imuData is None:
        retries -= 1
        imuData = imuQueue.tryGet()
        if imuData is None:
            # Wait for a small time before retrying
            await asyncio.sleep(0.1)

    if imuData:
        imuPackets = imuData.packets
        rVvalues = imuPackets[0].rotationVector
        print(rVvalues.i, rVvalues.j, rVvalues.k, rVvalues.real)
    else:
        self.logger.error("Failed to retrieve IMU data!")

Thoughts?
Jaka

blank

jakaskerl Thanks, but unfortunately, that doesn't yield anything either, even when giving it multiple seconds instead of 100ms. imuData simply stays empty.

Could it be a problem that I use the same pipeline for still images and IMU values?

jakaskerl

Hi blank
No, shouldn't be a problem I think.
Hmm, I don't see the link between imu output and xlinkOut input. That would mean you are not getting any values from the IMU anyway.

Thanks,
Jaka

blank

jakaskerl Hmm, I don't see the link between imu output and xlinkOut input.

Thanks, that was the problem. Adding imu.out.link(xlinkOutImu.input), the imuQueue started yielding measurements.

One more question. IfI only want to query the most recent result every x seconds (which is an eternity for the IMU), should I set imu.setMaxBatchReports(…) and the maxSize of imuQueue to low values? Maybe just 1?

jakaskerl

Hi blank
I guess it would make it easier since you would only get one value per report. You can also set the report rate if you want using enableIMUSensor(sensor, reportRate=some_number).

Thanks,
Jaka