Bug in rectification parameters generation leading to wrong P

[ferreram]

Hi there,

I have spotted a bug in StereoCameraParameters::generateRectificationParameters() leading to wrong rectification of images (at least with the euroc_camchain config file taken from voxblox).

Starting l.655 in src/camera_parameters.cpp,

  first_.setInputCameraParameters(
      first_.getInputPtr()->resolution(),
      T.inverse() * first_.getInputPtr()->T(), first_.getInputPtr()->K(),
      first_.getInputPtr()->D(), first_.getInputPtr()->distortionModel());
  second_.setInputCameraParameters(
      second_.getInputPtr()->resolution(),
      T.inverse() * second_.getInputPtr()->T(), second_.getInputPtr()->K(),
      second_.getInputPtr()->D(), second_.getInputPtr()->distortionModel());

here T is a 4x4 homogeneous matrix defined as Eigen::Matrix4d whose top-left 3x3 block represents an SO(3) rotation matrix. The inverse of T must therefore be computed as the inverse of an SO(3) matrix :

  Eigen::Matrix4d invT;
  invT.block<3,3>(0,0) = T.block<3,3>(0,0).transpose();

  first_.setInputCameraParameters(
      first_.getInputPtr()->resolution(),
      invT * first_.getInputPtr()->T(), first_.getInputPtr()->K(),
      first_.getInputPtr()->D(), first_.getInputPtr()->distortionModel());
  second_.setInputCameraParameters(
      second_.getInputPtr()->resolution(),
      invT * second_.getInputPtr()->T(), second_.getInputPtr()->K(),
      second_.getInputPtr()->D(), second_.getInputPtr()->distortionModel());

Without this fix, the resulting P's matrices are wrong and create flipped images with non aligned along the y axis.

I am running ubuntu 18.04 (ROS melodic) and encountered this issue with both Eigen 3.3.4 and Eigen 3.3.9.

[TheFrey222]

thank you, you saved my day