Get camera intrinsics from camera_info topic

[schornakj]

Implements #25 (@gavanderhoorn)

• Use ros::topic::waitForMessage when initializing the yak_ros node to get camera intrinsics published on the camera_info topic for the subscribed depth camera. If no topic is found, fall back to loading from ROS parameters instead.
• Change the simulated depth image publisher to use the image_transport::CameraPublisher, which publishes a camera_info topic. The default depth image topic is now sim_depth_camera/image, and camera info is published on sim_depth_camera/camera_info.
• Change demo.launch to use this new functionality. In particular note that the topic remaps have changed.

[gavanderhoorn]

Thanks for the PR @schornakj.

I didn't open #25 for you to fix it, but just wanted to get some input on whether something like that would fit with the library.

We'll test this and let you know.

[dave992]

Thank you for implementing this @schornakj! I have just tested this in our current setup using the Zivid One+ 3D camera and yak_ros now loads the information from the camera_info topic.

I am not too familiar yet with this type of camera's but the Zivid only publishes the camera_info when taking a picture. For our setup, this means that I have to trigger at least one capture as soon as the camera node starts or otherwise it will use the default parameters.

If this is the standard for this type of cameras, would it be an idea to check for the existence of the camera_matrix before using ros::topic::waitForMessage? If the camera_matrix parameter does not exist then wait for the CameraInfo message for a longer duration.

[schornakj]

I didn't open #25 for you to fix it, but just wanted to get some input on whether something like that would fit with the library.

It was a quick addition so it seemed worthwhile to just go ahead and do it.

Thank you for implementing this @schornakj! I have just tested this in our current setup using the Zivid One+ 3D camera and yak_ros now loads the information from the camera_info topic.

I am not too familiar yet with this type of camera's but the Zivid only publishes the camera_info when taking a picture. For our setup, this means that I have to trigger at least one capture as soon as the camera node starts or otherwise it will use the default parameters.

If this is the standard for this type of cameras, would it be an idea to check for the existence of the camera_matrix before using ros::topic::waitForMessage? If the camera_matrix parameter does not exist then wait for the CameraInfo message for a longer duration.

Good point, I think the ROS nodes for other single-shot 3D cameras like Photoneo's PhoXi work similarly.

We could check in this order:

1. Wait a short time for a message to appear on the camera_info topic. If we receive one, use the contents to initialize the camera intrinsics and image size and then set the Kinfu params.
2. If waitForMessage times out, try to load the camera_matrix, rows, and cols ROS params. If we find them, get their values and use them to set the Kinfu params.
3. If the camera_matrix, etc. params are not set for the yak_ros node, then wait indefinitely for a message to be received on the camera_info topic or for all the parameters to be set.

I don't think we'd want to continue initializing the node with default camera intrinsics: if the camera matrix is wrong the reconstruction will be heavily distorted, and if the resolution is wrong I think it might crash with an out-of-bounds exception at some point.

This is a situation where it would be beneficial to not be required to set camera intrinsics when the node initializes and instead tie them to each new incoming image, as described in this comment and this yak issue.

[dave992]

We could check in this order:

1. Wait a short time for a message to appear on the camera_info topic. If we receive one, use the contents to initialize the camera intrinsics and image size and then set the Kinfu params.
2. If waitForMessage times out, try to load the camera_matrix, rows, and cols ROS params. If we find them, get their values and use them to set the Kinfu params.
3. If the camera_matrix, etc. params are not set for the yak_ros node, then wait indefinitely for a message to be received on the camera_info topic or for all the parameters to be set.

Yeah, that sounds great. Just a question, what would be the benefit of keeping step 1. instead of skipping it altogether?

I don't think we'd want to continue initializing the node with default camera intrinsics: if the camera matrix is wrong the reconstruction will be heavily distorted, and if the resolution is wrong I think it might crash with an out-of-bounds exception at some point.

Initializing without correct intrinsics would indeed not make much sense.

This is a situation where it would be beneficial to not be required to set camera intrinsics when the node initializes and instead tie them to each new incoming image, as described in this comment and this yak issue.

When using a single camera with static intrinsics it may be overkill, but it is definitely a nice addition in a multi-camera setup or other situations with changing intrinsics.

[schornakj]

Yeah, that sounds great. Just a question, what would be the benefit of keeping step 1. instead of skipping it altogether?

I think it depends on how much we trust the user to set correct intrinsics as parameters vs. how much we trust the camera driver to publish accurate data on its camera_info topic. It would be reasonable to expect that a user who's trying to override the camera_info topic by explicitly setting parameters for the node is doing so with good reason, and that we'd want to let them do that without too much hassle.

Revised steps:

1. Try to load the camera_matrix, rows, and cols from ROS params. If we find them, get their values and use them to set the Kinfu params.

2. If the camera_matrix, etc. params are not set for the yak_ros node, then use waitForMessage to wait indefinitely for a message to be received on the camera_info topic or for all the parameters to be set.