Generating the unit vectors image from the intrinsic data stored on the O3D device

[flajolet]

Hello,
( I am using an O3D device with firmware version 1.30.5309)

I have gone through all the documentation I could find on the intrinsic calibration stored on the device (this github issue was particularly useful #108) but I still cannot find an answer to the following question:

How can I reconstruct the unit vector image from the intrinsic data stored on the device?

The goal being to record only this more compact representation of data and regenerate the unit vector image representation as needed.

I have tried many things and I am getting close but there is still a small different between the unit vector image I generate from the intrinsics data (well more specifically what is referred to as the "inverse" intrinsic calibration in this repository) and the unit vectors reported by the device.

Here is how far I've got so far:

• I've configured the device to not rectify the amplitude image and the distance image
• I've set all extrinsics parameters in the device configuration to 0.
• I've extracted the inverse intrinsic calibration from the device using the InverseIntrinsics function
• I've extracted the extrinsic calibration (optical frame to -> ifm frame) stored on my device using the Extrinsics() function to express the unit vectors I generate in the ifm frame (the unit vectors are reported in this frame if I understood correctly this unit test correctly).

Given that, I use the following python snippet to generate the unit vector image:

import image_geometry
import numpy
import math
from tf.transformations import quaternion_from_euler, quaternion_matrix
from sensor_msgs.msg import CameraInfo


cam_info_msg = CameraInfo()
cam_info_msg.height = 264
cam_info_msg.width= 352
cam_info_msg.distortion_model = "plumb_bob"
cam_info_msg.D = [-0.3871031701564789, 0.07685842365026474, 0.0, 0.0, 0.09532918781042099]
cam_info_msg.K = [353.70770263671875, 0.0, 178.9272003173828, 0.0, 353.70770263671875, 136.00311279296875, 0.0, 0.0, 1.0]
cam_info_msg.P = [353.70770263671875, 0.0, 178.9272003173828, 0.0, 0.0, 353.70770263671875, 136.00311279296875, 0., 0.0, 0.0, 1.0, 0.]
cam_info_msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
cam_info_msg.binning_x = 1
cam_info_msg.binning_y = 1


cam_model = image_geometry.PinholeCameraModel()
cam_model.fromCameraInfo(cam_info_msg)

optical_to_ifm_transform =  quaternion_matrix = quaternion_matrix(
    quaternion_from_euler(
        math.radians(0.200534582138), 0.,  math.radians(0.368382424116)))
# this is the optical -> ifm transform stored on my device

unit_vectors = numpy.zeros((cam_info_msg.height, cam_info_msg.width, 3))
for i in xrange(cam_info_msg.height):
    for j in xrange(cam_info_msg.width):
        rectified_coords = cam_model.rectifyPoint((float(j) + 0.5, float(i) + 0.5))
        vector = cam_model.projectPixelTo3dRay((rectified_coords[0] - 0.5, rectified_coords[1] - 0.5))
        unit_vectors[i, j, 0] = vector[0]
        unit_vectors[i, j, 1] = vector[1]
        unit_vectors[i, j, 2] = vector[2]
        unit_vectors[i, j, :] = numpy.dot(optical_to_ifm_transform[0:3,0:3], unit_vectors[i, j, :])

The coordinates unit vectors I get differ from the original on average by ~ 0.001 and at most by0.002.

[theseankelly]

I don't have an answer to your primary question off the top of my head; let me dig in and get back to you.

In the meantime is your goal to minimize network utilization (by not needing to transfer the UVEC image array on each frame) or storage (not needing to keep a copy of the unit vectors around in memory)? If network, you could always create the ifm3d::FrameGrabber with the UVEC flag specfied, cache the vectors, and then reset/re-create a new FrameGrabber without the flag set.

[theseankelly]

Actually I just noticed that your sample code is ROS based. Are you using the ifm3d-ros bindings? If so, the idea I mentioned is already implemented. Unit vectors are pulled from the device once at init, published to a latched topic, and then the FrameGrabber is reconfigured to not stream unit vectors any longer.

Implementation: https://github.com/ifm/ifm3d-ros/blob/36850c79b987985b45132f86a2b615da6913924e/src/camera_nodelet.cpp#L540

[theseankelly]

Hey @GreNait do we have any official application notes describing how to derive the unit vectors from the calibration information?

[flajolet]

Awesome, thanks for the quick reply!

Actually I just noticed that your sample code is ROS based. Are you using the ifm3d-ros bindings? If so, the idea I mentioned is already implemented. Unit vectors are pulled from the device once at init, published to a latched topic, and then the FrameGrabber is reconfigured to not stream unit vectors any longer.

Very good point! and yes that's what I do currently to minimize the network overhead.

In the meantime is your goal to minimize network utilization (by not needing to transfer the UVEC image array on each frame) or storage (not needing to keep a copy of the unit vectors around in memory)? If network, you could always create the ifm3d::FrameGrabber with the UVEC flag specfied, cache the vectors, and then reset/re-create a new FrameGrabber without the flag set

My goals are:

1. minimize network utilization (and you've completely addressed this point)
2. handling the O3D camera just like any other camera (with an extra depth channel), this would have the big advantage of being able to reuse a lot of boilerplate code as is (i.e. without adding if else statement for dealing with the unit_vector image representation instead of the sensor_msgs.msg/CameraInfo representation of the intrinsics).

[theseankelly]

Cool, glad that was useful.

Your point is understood -- let me circle back internally and see if we have any documentation (already) on the computation of unit vectors. If not it might be a good candidate to put a sample together for, like we did for image rectification.

There's also this issue floating around on our ROS driver that I haven't gotten to...would this be useful? If so I'll consider this to be another vote in favor of doing that work sooner.

[flajolet]

Your point is understood -- let me circle back internally and see if we have any documentation (already) on the computation of unit vectors. If not it might be a good candidate to put a sample together for, like we did for image rectification.

Thanks!

There's also this issue floating around on our ROS driver that I haven't gotten to...would this be useful? If so I'll consider this to be another vote in favor of doing that work sooner.

I ended up implementing something along these lines. I'll try to submit a PR if that's ok with you.

[theseankelly]

Hey @flajolet -- @GreNait is still checking internally on the intrinsics question.

In the meantime, always happy to review and accept community PRs.

[flajolet]

Hey @flajolet -- @GreNait is still checking internally on the intrinsics question.

Thanks for following up!

In the meantime, always happy to review and accept community PRs.

Great, I'll try to put something together.

[theseankelly]

Hey @flajolet -- sorry for the delay on this topic. Here's an example jupyter notebook demonstrating the process of computing the unit vectors (and then on to the cartesian data) from the camera's intrinsics parameters. Tested on an O3D303.

[flajolet]

Thank you so much, this is very helpful!

[theseankelly]

Glad it's helpful.

By the way, while working on that document I also saw errors on the order of 1e-3 in my computed unit vectors versus the values read from the camera. But, as you can see when projecting back to cartesian, this error doesn't seem to be enough to make a significant difference. Might just be standard floating point precision stuff.

I'm going to close this, feel free to re-open or create a new issue as needed.