stereo_undistort_node: rectification fails to generate valid P matrices

[GoosebumpsFactory]

Thank you for releasing this useful package. I am seeing some unexpected behavior in stereo_undistort_node, which might be due to improper usage by me or possibly a bug in the code. I am seeing P matrices generated by stereo_undistort_ node with non-zero y and z offsets.

Here is a reproduction test that should help reproduce the issue at your end.
Usage: roslaunch image_undistort rect_test.launch

The folder on google drive contains:

• static.bag: the example dataset that comes with Kailbr
• camchain-static.yaml: generated using a fresh build of Kailbr/master; I have also tried Kalibr-CDE
• rect_test.lauch: a launch file that produces the following error at my end:
  [ERROR] [1538875392.391423846]: Image P matrices must match (excluding x offset)
[ERROR] [1538875392.391476697]: Camera info processing failed, skipping disparity generation

When I observe the rectified images using the following command they appear to be flipped in y-axis: rqt_image_view /test/rect/first/image

Any pointers on debugging this issue would be really helpful. Thanks.

[GoosebumpsFactory]

Sample output from the following command: rostopic echo /test/rect/second/camera_info -n1

header: 
  seq: 185
  stamp: 
    secs: 1402057030
    nsecs: 659047000
  frame_id: "second_camera_rect"
height: 494
width: 892
distortion_model: "equidistant"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [461.917116406338, 0.0, 446.0, 0.0, 461.917116406338, 247.0, 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: [461.917116406338, 0.0, 446.0, -50.89295743426754, 0.0, 461.917116406338, 247.0, 0.3013182369420908, 0.0, 0.0, 1.0, 7.012326575932563e-10]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
---

[ZacharyTaylor]

Had a quick look and all the inputs appear reasonable so this is likely a bug, I will try go through it in more detail today or tomorrow

[GoosebumpsFactory]

Thanks for looking into the issue.

[GoosebumpsFactory]

Hey @ZacharyTaylor - just wanted to let you know I can no longer reproduce this issue at my end. I am not sure what changed as no code seems to have changed. Maybe a clean build of the catkin workspace is what fixed it. Not sure. Sample output from the following command: rostopic echo /test/rect/second/camera_info -n1

header: 
  seq: 24
  stamp: 
    secs: 1402056976
    nsecs: 992917000
  frame_id: "second_camera_rect"
height: 494
width: 892
distortion_model: "equidistant"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [461.917116406338, 0.0, 446.0, 0.0, 461.917116406338, 247.0, 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: [461.917116406338, 0.0, 446.0, -50.89384973467052, 0.0, 461.917116406338, 247.0, 0.0, 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
---

Notice that the y and z offsets in the P-matrix are now zero. Sorry - not sure what changed.

[GoosebumpsFactory]

FYI: There is some evidence to suggest the issue is caused by using compiler tools (gcc,cpp,g++) version 7. When I switch to version 6, the issue goes away. I used the followed command to force catkin build to use version 6 tools instead of 7.

update-alternatives --config gcc
update-alternatives --config g++
update-alternatives --config cpp

On my machine I have two alternatives for each (version 6 & 7) but they are all set to version 6.

[JohanVer]

I can confirm @GoosebumpsFactory. I ran into the exact same problem and solved it by downgrading gcc from version 7 to 6. I followed link for changing the version.

[jokla]

Any idea on how to solve this issue without downgrading gcc?

[eugenelyj]

Any idea on how to solve this issue without downgrading gcc?

It seems that this problem still exist.

[HongLouyemeng]

Any idea on how to solve this issue without downgrading gcc?

It seems that this problem still exist.

catkin build -DCMAKE_C_COMPILER=/usr/bin/gcc-6 -DCMAKE_CXX_COMPILER=/usr/bin/g++-6

[meyerjo]

Can confirm that the issue still exists

FROM osrf/ros:kinetic-desktop-full
#FROM osrf/ros:noetic-desktop-full

ENV QT_X11_NO_MITSHM=1
ENV XAUTHORITY=/tmp/.docker.xauth
ENV NVIDIA_VISIBLE_DEVICES=all
ENV NVIDIA_DRIVER_CAPABILITIES=all

RUN apt-get install -y curl
RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
RUN apt-get update && apt-get install -y libnlopt-dev git
RUN apt-get install -y ros-$ROS_DISTRO-catkin
RUN if [ "$ROS_DISTRO" = "noetic" ]; then apt-get install -y python3-catkin-tools python3-osrf-pycommon; else apt-get install -y python-catkin-tools; fi

WORKDIR /build
RUN git clone https://github.com/stevengj/nlopt.git && mkdir nlopt/build && cd nlopt/build && cmake .. && make && make install

WORKDIR /code/src
RUN catkin init
RUN mkdir image_undistort # new
COPY image_undistort image_undistort/

SHELL ["/bin/bash", "-c"]
RUN mkdir -p /opt/ros/$ROS_DISTRO/config/ && echo "log4j.logger.ros=DEBUG" >> /opt/ros/$ROS_DISTRO/config/rosconsole.config

RUN source /ros_entrypoint.sh && cd /code/ && catkin build
RUN source /ros_entrypoint.sh && cd /code/ && source /code/devel/setup.bash && rospack list
CMD source /ros_entrypoint.sh && source /code/devel/setup.bash && roslaunch /launch/densedepth.launch

with noetic as version it fails. However, if you switch to kinetic it works. (This obviously includes a downgrade in gcc)

Two observations:

• In noetic the rectified image still gets published. However, it is upside down.
• In my case P[7] and P[11] differ depending on the ROS version. For cam0 this field is 0 in any case. However, in kinetic they are -1.4188784202809145e-15 and -1.734723475976807e-18 but in noetic 5.805111461987429 and 8.205501772825391e-07 (both large enough to cause a failure).

[meyerjo]

I took another look at it. It appears to be caused by a numerical instability in the computation of T.inverse()

image_undistort/src/camera_parameters.cpp

Line 657 in a8fa9cd

T.inverse() * first_.getInputPtr()->T(), first_.getInputPtr()->K(),

.
In ROS Kinetic / Ubuntu 16.04 it returns a matrix like that one
0.9999524852420787, 0.003604255274427447, -0.009057405925931806, -0, -0.003604403123952235, 0.9999935041179618, -0, -0, 0.009057347090091339, 3.264654221433209e-05, 0.9999589808576612, -0, -0, -0, -0, 1.

However, in ROS Noetic / Ubuntu 20.04
0.999952485242079, -0.00360425527442745, -0.00905740592593181, 0, -0.00360440312395223, -0.999993504117962, -0, -0, 0.00905734709009134, -3.26465422143321e-05, 0.999958980857661, 0, -0, 0, -0, -1 is returned. Note the quite substantial flips in the sign. (Both inputs were the same to some decimal point).

I tried to find a minimal example showing the problem for some time. However, it just seems to be the compile flag. Compiling image_undistort is done with the -Ofast flag set. I set the flag manually to -O3 and everything is working fine.

You can see https://github.com/meyerjo/minimal_eigen_example for a minimal example and the respective output both in ubuntu 16.04 and in ubuntu 20.04 with the different optimization flags set.

[meyerjo]

One addition: another solution would be to update Eigen to 3.4.0. This seems to at least solve this particular problem of the matrix inversion with the -Ofast flag (did not check the complete output).

[Youras]

I am running ros noetic on ubuntu 20.04. and still cannot solve this issue, even with the above mentioned hot fixes.
I tried to build again with the -O3 flag and I tried with downgrading to g++-6 making use of update-alternatives.

D ist still all zero.

And I also noticed that even though my calibration with kalibr applied omni radtan model, the output of stereo_undistort gives equidistant as distortion model in the camera info. In the code I noticed that when it comes to the omni-radtan case, it is expecting 6 intrinsic parameters. But according to kalibr supported models omni has only 5 intrinsics parameters and radtan has 4 distortion coeffs.

undistorter.cpp :

case DistortionModel::OMNIRADTAN: {
      // Split out parameters for easier reading
      const double& xi = D[0];
      const double& k1 = D[1];
      const double& k2 = D[2];
      const double& k3 = D[5];
      const double& p1 = D[3];
      const double& p2 = D[4];