Expose stereo extrinsics

[mindThomas]

1. Expose the stereo extrinsics correctly within the Projection matrix
2. Change the reported distortion model to equidistant when using Kannala Brandt model

These change will ensure that the camera_info includes the correct information expected by e.g. the stereo_undistort node in: https://github.com/ethz-asl/image_undistort

[doronhi]

Thank you for this PR.
You present here with 2 issues. The first one, regarding the the equidistant distortion model, is true and is was dealt with, in #1225
As for the extrinsics, that is a complex issue. It was discussed shortly here: #901 . It may need to involve removing inra2_frame_id completely and treating the IR as a single sensor. You can follow the links in #901 for better explanation. Since it involves a sort of API change and it's not clear that its a practiced convention, I didn't rush to change it.

[mindThomas]

@doronhi Why are you suggesting that it would require an API change?
I haven't tested the #901 PR, but it seems like this PR would achieve the same thing. This change of including the extrinsics within the camera_info is a necessity for stereo rectification modules such as stereo_image_proc, but I don't see why this change would require an API change?

[doronhi]

If I understand correctly, what you've done here complies with the definition of cameraInfo.
If I understand @tykurtz correctly, since now R reflects a rotation to frame_id "infra1", in means that infra2 sensor frame_id's is also "infra1".
That means modifying the frame_id on the "/camera/infra2/image_rect_raw" and maybe the "/tf" too.
Although it may be the right thing to do, it means changing the way users have been so far treating this frame.
That's what I meant by "API change".

If indeed, a change in frame_id is needed, I would like to have it in the same PR.
If not, we can accept your PR (removing the distortion model part) and dismiss #901 as included in this one.

So far I did not manage to expert myself on this subject and therefor, a clear "official" reference, would help me commit. @tykurtz? @pavloblindnology ? @stwirth ? I would appreciate help. What is the the official, acceptable, convention regarding the frame id's of a stereo camera?

[doronhi]

Hi,
Without further response I would like to approve this PR and keep the decision regarding the frame id of sensor infrared2 for a later date.
@mindThomas , could you please rebase this PR?

[doronhi]

@mindThomas , would you be interested in checking @shirbarzel's experience and trouble with this PR? #1310

[doronhi]

@mindThomas ,
Sorry for getting to this PR so sporadically.
Thank you for taking the time and rebasing it.

[pavloblindnology]

If I understand correctly, what you've done here complies with the definition of cameraInfo.
If I understand @tykurtz correctly, since now R reflects a rotation to frame_id "infra1", in means that infra2 sensor frame_id's is also "infra1".
That means modifying the frame_id on the "/camera/infra2/image_rect_raw" and maybe the "/tf" too.
Although it may be the right thing to do, it means changing the way users have been so far treating this frame.
That's what I meant by "API change".

If indeed, a change in frame_id is needed, I would like to have it in the same PR.
If not, we can accept your PR (removing the distortion model part) and dismiss #901 as included in this one.

So far I did not manage to expert myself on this subject and therefor, a clear "official" reference, would help me commit. @tykurtz? @pavloblindnology ? @stwirth ? I would appreciate help. What is the the official, acceptable, convention regarding the frame id's of a stereo camera?

Somehow I missed this PR discussion.
Having right camera frame_id equal to that of left camera is required by StereoCameraModel from image_geometry, stereo_image_proc which is based on the latter, and RViz Camera Display plugin. Probably there are other packages too.
So, it seems like it's a standard for stereo processing. And it sounds reasonable as right camera projection matrix P with P(0,3) set up corresponds to projecting from left camera frame.
As for TF, we can still publish right camera frame - for the purpose of URDF visualization, for example. It just won't be used by right image topic.
So, we can for example, add next line in updateExtrinsicsCalibData:
_camera_info[right].header.frame_id = _camera_info[left].header.frame_id;
and in publishFrame use camera info frame_id as image frame_id (actually, I don't know why it's not done like this, why do we need optical_frame_id at all);
img->header.frame_id = cam_info.header.frame_id;

And P(0,3) should be negative, as I mentioned here.

Another problem to note is that updateStreamCalibData appears 2 times in the code, the 2nd time in publishFrame

realsense-ros/realsense2_camera/src/base_realsense_node.cpp

Line 2413 in aaf6df2

updateStreamCalibData(f.get_profile().as<rs2::video_stream_profile>());

which may reset changes done by updateExtrinsicsCalibData. So it needs to be called here too.

[mindThomas]

Another problem to note is that updateStreamCalibData appears 2 times in the code, the 2nd time in publishFrame which may reset changes done by updateExtrinsicsCalibData. So it needs to be called here too.

This is a good point.
If intrinsics are changed, calling that second updateStreamCalibData, the extrinsics will also have to be recomputed. This will however require a change to the current implementation since the two profiles are not stored so there is currently no way of recomputing the extrinsics within the publishFrame of a single frame.

[pavloblindnology]

This is a good point.
If intrinsics are changed, calling that second updateStreamCalibData, the extrinsics will also have to be recomputed. This will however require a change to the current implementation since the two profiles are not stored so there is currently no way of recomputing the extrinsics within the publishFrame of a single frame.

BTW, no such problem in #901 :)