Support for camera's calibration from input topic

[AlexeyMerzlyakov]

• Currently, camera calibration parameters are passing to openvslam_ros through a setting file only. Adding an ability to read incoming topic provided by a camera ROS driver would be a good point to work with.

• It could be checked whether OpenVSLAM does support possible distortion models, coefficients formats, etc... from incoming topics. For example, Gazebo Realsense for TB3 is publishing {d, k, r, p} calibration coefficients while input file for OpenVSLAM has another camera coefficients format {fx/fy, cx/cy, k, p}, so it probably need to add a conversion from one format to another.

[SteveMacenski]

I'd also investigate how other ROS nodes do this. There are a few methods for getting camera calibration files and I'm honestly not sure which is the best / most "standard". Some are using the topic, some are reading directly from the output of the camera calibration node (which contains the same info as the CameraInfo msg type), some are setting parameters in a config file like any other.

Maybe a param for the "camera calibration filepath" to get the params from a camera calibration file and/or use the topic with use_topic-esk param? The big question on that one is how to support multiple file structures (if we expect multiple to try to be given) or point to the format we expect as a demo / output of a particular node

[saching13]

I will take a look at Gazebo's for TB3 and see what that openVSLAM supports.

while input file for openvslam has another camera coefficients format {fx/fy, cx/cy, k, p}, so it probably need to add a conversion from one format to another.

I saw this conversion. And I think I saw 3 model types (Not sure) Will cross-check and see where and what conversions are needed.

like steve mentioned I will take a look at how other camera's ROS drivers handle this and see what's the best approach.
Will add more findings here.

[SteveMacenski]

I'd look at rtabmap as an example, see what they do

[saching13]

Sorry for the delay here.
Openvslam allows FIsheye, Perspective, equirectangular and RadialDivision formats.
And as of loading the calibration it is loaded to the system object at the initialization.
And tracking uses this at every frame in feature detection.

And as of RTAB-MAP they register to camera_info along with the image topics and each camera_info message received is converted to their Camera object and passed along with the image for processing. you can see one of the instances here

So if we decide to use camera_info then we need to add function to openvlsam::system to update the camera object at each instance before image is processed.
And another concern in doing this is that when map is created at the end this camera is also stored in the DB along with the map. So do we store all the incomings or just the last ? I am not sure what is the use of storing it yet (still digging into that part).

And as of the Gazebo Realsense for TB3 Plugin. It is same as perspective in openvslam. So no issues there.

[SteveMacenski]

So if we decide to use camera_info then we need to add function to openvlsam::system to update the camera object at each instance before image is processed.

@ymd-stella does that seem reasonable? Is there any assumption in the code that its the same parameters camera that is across the board? What if there's an auto-focus camera in use? I think this would involve storing the important camera parameters along with the keyframe (if necessary, or just rectify at start so none of that matters).

[AlexeyMerzlyakov]

How strong calibration parameters could change in case of auto-focus was triggered? As I understand, now OpenVSLAM loads some averaged parameters on start, counting them as a first approximation to be constant for the whole session.

[ymd-stella]

@SteveMacenski

does that seem reasonable?

I don't think sensor_msgs/CameraInfo is appropriate for representing the camera model supported by OpenVSLAM. If you do use topics, you should design a different type. Also, I have doubts about using topics for camera parameters. Shouldn't we use service or parameter API?

Is there any assumption in the code that its the same parameters camera that is across the board? What if there's an auto-focus camera in use?

I had never thought of using a camera with changing parameters.

I think this would involve storing the important camera parameters along with the keyframe

It is stored as a reference. This is also the reason why the camera should have fixed parameters.
https://github.com/OpenVSLAM-Community/openvslam/blob/00cdcc6d5c979c0ebd461f59974b4aa43e3c1a2b/src/openvslam/data/keyframe.h#L229

@AlexeyMerzlyakov
I do not understand the purpose of this ticket. What is the problem with the current implementation?

As I understand, now OpenVSLAM loads some averaged parameters on start, counting them as a first approximation to be constant for the whole session.

Where are you talking about?

[AlexeyMerzlyakov]

I do not understand the purpose of this ticket. What is the problem with the current implementation?

Usually, the ROS camera driver is being publishing camera calibration parameters in a camera info topic. The main goal of this ticket - is to support the camera calibration parameters delivered by info topics (like RTABMap-ROS does) in addition to input file. This could be convenient when running OpenVSLAM as a SLAM node as a part of whole navigation2 stack, where we could tune to what camera driver is published. Also, the structure of OpenVSLAM calibration format differs from that published in CameraInfo messages, so we also should support a conversion from one {d, k, r, p} to OpenVSLAM-compatible {fx/fy, cx/cy, k, p} coefficients format.
To summarize, the intention of this ticket is to add this new option in addition to current OpenVSLAM-ROS functionality, not breaking the rest.

Where are you talking about?

During the camera operation such things like auto-focus could affect camera calibration parameters (e.g. focal length was changed when camera tuned to closer objects). Should this be taken into account and can calibration parameters be changed during the run-time in a camera info messages, that is my initial question.

[SteveMacenski]

The "typical ROS-y" way of working with cameras is to have a synchronized topic camera_info along side the images to use for calibration. The image transport tools within ROS provide this automatically and its a pretty standard interface most things in the visual-ROS ecosystem use. Your service / parameter question is a valid one, but I'm just thinking about the current design of much of the rest of the ecosystem.

If you're saying, though, that CameraInfo doesn't contain the right fields to describe the openVSLAM models, then this isn't a valid option and we can drop that part (though it looks like it is from glancing at example configuration files https://github.com/OpenVSLAM-Community/openvslam_ros/blob/ros2/example/kitti/KITTI_mono_03.yaml#L7-L26)

I had never thought of using a camera with changing parameters.

Its actually becoming increasingly common for robots to use things like smart phone cameras and other auto-focusing tools rather than fixed-lens ML cameras only. This in general isn't a problem if the pipeline rectifies the images on receipt and then throws them through the system (because then all of the stuff in openVSLAM would be calibration independent). It just requires taking in the current state of the camera calibration to use for that initial rectification.

---

There isn't strictly a problem with the current implementation, but its extremely non-standard and we'd like to make the openvslam_ros wrapper be as "ROS-y" as possible so that it follows the best practices and conventions of ROS nodes, visual-ROS nodes, and localization nodes. The yaml files here are non-ROS yaml files, so at bare minimum we'd want to make all of those parameters ROS parameters and read in as ROS parameters so that users can add this to their master configuration files with the rest of their nodes. Additionally, it allows them to do things like remap parameters (which in this context I think is a bad idea) and use all of the other tools in ROS regarding parameters available. This is why we're also talking about the /initialpose stuff -- these are all things users and existing tools expect to exist for any given SLAM system.

We're mostly working here on using best practices / conventions / tools, where possible, since our aim is to make this the #1 integration into VSLAM for ROS2.

[saching13]

So, what's the final decision here?

I can start making an initial draft of additional functions to add for handling the conversion and passing it to openVSLAM along with frame using an overloading function maybe...

[AlexeyMerzlyakov]

If camera_info calibration parameters could be converted to OpenVSLAM-compatible ones, there is OK to go. This could be handled by adding an additional ROS-parameter to OpenVSLAM node which will trigger to use input calibration files or camera info messages.

As I understand from above, currently we are putting off dynamically changing calibration coefficients, since it is not supported by current implementation of OpenVSLAM. However, this could be resolved by camera ROS-driver providing a rectified input images. So, for now, let's assume one of the calibration coefficients from camera_info and use rectified images if provided.

Finally, we do not need to add any new information or change map DB file produced by OpenVSLAM.

@ymd-stella, @SteveMacenski, please correct me if I wrong.

[AlexeyMerzlyakov]

@saching13, are there any updates or news on it?

[saching13]

@AlexeyMerzlyakov Sorry. I don't have an update yet. Have been a little overwhelmed at work for the past few weeks and couldn't allocate time to this.

Will be working on it this weekend. Will post an update next week.

[saching13]

I went through the code over the weekend and I can think of two ways to try this out.

• Add a CameraInfoToVslamCamera Converter function and initialize the vslam system in the subscription callback when the first image and camera_info comes in. This should be simple from implementation perspective and with no changes to underlying OpenVSLAM System.
• Another option would be to have an another constructor in OpenVSLAM System and Initialize the camera and the Camera Database using an additional function like startup() when the first camera_info message comes in.

Thoughts ?

[ymd-stella]

I think the latter is better.

[ryleymcc]

So how is this coming along. It would be really nice to do some experiments with this package on my realsense robot.
This is the fisheye2/camera_info topic form my realsense t265. Frame id is fisheye1 not fisheye 2

header: 
  seq: 1040
  stamp: 
    secs: 1634694730
    nsecs: 852093697
  frame_id: "t265_fisheye1_optical_frame"
height: 800
width: 848
distortion_model: "equidistant"
D: [-0.009274626150727272, 0.04619137942790985, -0.043395038694143295, 0.00802248902618885, 0.0]
K: [286.1571960449219, 0.0, 423.5339050292969, 0.0, 286.00799560546875, 401.7677917480469, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [286.1571960449219, 0.0, 423.5339050292969, -18.31967544555664, 0.0, 286.00799560546875, 401.7677917480469, -0.1004444807767868, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
---

This the topic for fisheye1. They are the same.

header: 
  seq: 3113
  stamp: 
    secs: 1634694960
    nsecs: 616133690
  frame_id: "t265_fisheye1_optical_frame"
height: 800
width: 848
distortion_model: "equidistant"
D: [-0.009274626150727272, 0.04619137942790985, -0.043395038694143295, 0.00802248902618885, 0.0]
K: [286.1571960449219, 0.0, 423.5339050292969, 0.0, 286.00799560546875, 401.7677917480469, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [286.1571960449219, 0.0, 423.5339050292969, -18.31967544555664, 0.0, 286.00799560546875, 401.7677917480469, -0.1004444807767868, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
---

[ryleymcc]

maybe you could use this as a base. https://github.com/mindThomas/image_undistort/tree/t265_stereo_undistort