src/frontend/StereoFrame.cpp
/* -------------------------------------------------------------------------- */
// note also computes the rectification maps
// TODO(Toni): this should be done much earlier and only once...
note also computes the rectification maps
TODO(Toni): this should be done much earlier and only once
note also computes the rectification maps TODO ( Toni ) : this should be done much earlier and only once
void StereoFrame::computeRectificationParameters(
    CameraParams *left_cam_params, CameraParams *right_cam_params,
    gtsam::Pose3 *B_Pose_camLrect) {
  CHECK_NOTNULL(left_cam_params);
  CHECK_NOTNULL(right_cam_params);
  CHECK_NOTNULL(B_Pose_camLrect);
  cv::Mat L_Rot_R, L_Tran_R;
  gtsam::Pose3 camL_Pose_camR = (left_cam_params->body_Pose_cam_)
                                    .between(right_cam_params->body_Pose_cam_);
  boost::tie(L_Rot_R, L_Tran_R) =
      UtilsOpenCV::Pose2cvmats(camL_Pose_camR.inverse());
  CameraParams &left_camera_info = *left_cam_params;
  CameraParams &right_camera_info = *right_cam_params;
  cv::Mat P1, P2, Q;
  if (left_camera_info.distortion_model_ == "radtan" ||
      left_camera_info.distortion_model_ == "radial-tangential") {
    VLOG(10) << "Stereo camera distortion for rectification: radtan";
    cv::stereoRectify(
        left_camera_info.K_,
        left_camera_info.distortion_coeff_,
        right_camera_info.K_,
        right_camera_info.distortion_coeff_,
        left_camera_info.image_size_,
        L_Rot_R,
        L_Tran_R,
        left_camera_info.R_rectify_, right_camera_info.R_rectify_, P1, P2, Q);
  } else if (left_camera_info.distortion_model_ == "equidistant") {
    VLOG(10) << "Stereo camera distortion for rectification: equidistant";
    cv::fisheye::stereoRectify(left_camera_info.K_,
                               left_camera_info.distortion_coeff_,
                               right_camera_info.K_,
                               right_camera_info.distortion_coeff_,
                               left_camera_info.image_size_,
                               L_Rot_R,
                               L_Tran_R,
                               left_camera_info.R_rectify_,
                               right_camera_info.R_rectify_,
                               P1,
                               P2,
                               Q,
                               cv::CALIB_ZERO_DISPARITY);
  } else {
    LOG(ERROR)
        << "Stereo camera distortion model not found for stereo rectification!";
  }
  VLOG(10) << "RESULTS OF RECTIFICATION: \n"
           << "left_camera_info.R_rectify_\n"
           << left_camera_info.R_rectify_ << '\n'
           << "right_camera_info.R_rectify_\n"
           << right_camera_info.R_rectify_;
  const gtsam::Rot3 &camL_Rot_camLrect =
      UtilsOpenCV::cvMatToGtsamRot3(left_camera_info.R_rectify_).inverse();
  gtsam::Pose3 camL_Pose_camLrect(camL_Rot_camLrect, gtsam::Point3::Zero());
  *B_Pose_camLrect =
      left_camera_info.body_Pose_cam_.compose(camL_Pose_camLrect);
  const gtsam::Rot3 &camR_Rot_camRrect =
      UtilsOpenCV::cvMatToGtsamRot3(right_camera_info.R_rectify_).inverse();
  gtsam::Pose3 camR_Pose_camRrect(camR_Rot_camRrect, gtsam::Point3());
  gtsam::Pose3 B_Pose_camRrect =
      (right_camera_info.body_Pose_cam_).compose(camR_Pose_camRrect);
  gtsam::Pose3 camLrect_Pose_calRrect =
      B_Pose_camLrect->between(B_Pose_camRrect);
  LOG_IF(FATAL,
         gtsam::Rot3::Logmap(camLrect_Pose_calRrect.rotation()).norm() > 1e-5)
      << "camL_Pose_camR log: "
      << gtsam::Rot3::Logmap(camL_Pose_camR.rotation()).norm() << '\n'
      << "camLrect_Pose_calRrect log: "
      << gtsam::Rot3::Logmap(camLrect_Pose_calRrect.rotation()).norm() << '\n'
      << "Vio constructor: camera poses do not seem to be rectified (rot)";
  LOG_IF(FATAL, fabs(camLrect_Pose_calRrect.translation().y()) > 1e-3 ||
                    fabs(camLrect_Pose_calRrect.translation().z()) > 1e-3)
      << "Vio constructor: camera poses do not seem to be rectified (tran) \n"
      << "camLrect_Poe_calRrect: " << camLrect_Pose_calRrect;
  if (left_camera_info.distortion_model_ == "radtan" ||
      left_camera_info.distortion_model_ == "radial-tangential") {
    VLOG(10) << "Left camera distortion: radtan";
    cv::initUndistortRectifyMap(left_camera_info.K_,
                                left_camera_info.distortion_coeff_,
                                left_camera_info.R_rectify_,
                                P1,
                                left_camera_info.image_size_,
                                CV_32FC1,
                                left_camera_info.undistRect_map_x_,
                                left_camera_info.undistRect_map_y_);
  } else if (left_camera_info.distortion_model_ == "equidistant") {
    VLOG(10) << "Left camera distortion: equidistant";
    cv::fisheye::initUndistortRectifyMap(left_camera_info.K_,
                                         left_camera_info.distortion_coeff_,
                                         left_camera_info.R_rectify_,
                                         P1,
                                         left_camera_info.image_size_,
                                         CV_32F,
                                         left_camera_info.undistRect_map_x_,
                                         left_camera_info.undistRect_map_y_);
  } else {
    LOG(ERROR) << "Camera distortion model not found for left camera!";
  }
  if (right_camera_info.distortion_model_ == "radtan" ||
      right_camera_info.distortion_model_ == "radial-tangential") {
    VLOG(10) << "Right camera distortion: radtan";
    cv::initUndistortRectifyMap(right_camera_info.K_,
                                right_camera_info.distortion_coeff_,
                                right_camera_info.R_rectify_, P2,
                                right_camera_info.image_size_, CV_32FC1,
                                :
                                right_camera_info.undistRect_map_x_,
                                right_camera_info.undistRect_map_y_);
  } else if (right_camera_info.distortion_model_ == "equidistant") {
    VLOG(10) << "Right camera distortion: equidistant";
    cv::fisheye::initUndistortRectifyMap(right_camera_info.K_,
                                         right_camera_info.distortion_coeff_,
                                         right_camera_info.R_rectify_,
                                         P2,
                                         right_camera_info.image_size_,
                                         CV_32F,
                                         :
                                         right_camera_info.undistRect_map_x_,
                                         right_camera_info.undistRect_map_y_);
  } else {
    LOG(ERROR) << "Camera distortion model not found for right camera!";
  }
  left_camera_info.P_ = P1;
  right_camera_info.P_ = P2;
  VLOG(10) << "Storing undistRect maps and other rectification parameters!";
}
void StereoFrame :: computeRectificationParameters ( CameraParams * left_cam_params , CameraParams * right_cam_params , gtsam :: Pose3 * B_Pose_camLrect ) { CHECK_NOTNULL ( left_cam_params ) ; CHECK_NOTNULL ( right_cam_params ) ; CHECK_NOTNULL ( B_Pose_camLrect ) ; cv :: Mat L_Rot_R , L_Tran_R ; gtsam :: Pose3 camL_Pose_camR = ( left_cam_params -> body_Pose_cam_ ) . between ( right_cam_params -> body_Pose_cam_ ) ; boost :: tie ( L_Rot_R , L_Tran_R ) = UtilsOpenCV :: Pose2cvmats ( camL_Pose_camR . inverse ( ) ) ; CameraParams & left_camera_info = * left_cam_params ; CameraParams & right_camera_info = * right_cam_params ; cv :: Mat P1 , P2 , Q ; if ( left_camera_info . distortion_model_ == " " || left_camera_info . distortion_model_ == " " ) { VLOG ( 10 ) << " " ; cv :: stereoRectify ( left_camera_info . K_ , left_camera_info . distortion_coeff_ , right_camera_info . K_ , right_camera_info . distortion_coeff_ , left_camera_info . image_size_ , L_Rot_R , L_Tran_R , left_camera_info . R_rectify_ , right_camera_info . R_rectify_ , P1 , P2 , Q ) ; } else if ( left_camera_info . distortion_model_ == " " ) { VLOG ( 10 ) << " " ; cv :: fisheye :: stereoRectify ( left_camera_info . K_ , left_camera_info . distortion_coeff_ , right_camera_info . K_ , right_camera_info . distortion_coeff_ , left_camera_info . image_size_ , L_Rot_R , L_Tran_R , left_camera_info . R_rectify_ , right_camera_info . R_rectify_ , P1 , P2 , Q , cv :: CALIB_ZERO_DISPARITY ) ; } else { LOG ( ERROR ) << " " ; } VLOG ( 10 ) << " \n " << " \n " << left_camera_info . R_rectify_ << ' \n ' << " \n " << right_camera_info . R_rectify_ ; const gtsam :: Rot3 & camL_Rot_camLrect = UtilsOpenCV :: cvMatToGtsamRot3 ( left_camera_info . R_rectify_ ) . inverse ( ) ; gtsam :: Pose3 camL_Pose_camLrect ( camL_Rot_camLrect , gtsam :: Point3 :: Zero ( ) ) ; * B_Pose_camLrect = left_camera_info . body_Pose_cam_ . compose ( camL_Pose_camLrect ) ; const gtsam :: Rot3 & camR_Rot_camRrect = UtilsOpenCV :: cvMatToGtsamRot3 ( right_camera_info . R_rectify_ ) . inverse ( ) ; gtsam :: Pose3 camR_Pose_camRrect ( camR_Rot_camRrect , gtsam :: Point3 ( ) ) ; gtsam :: Pose3 B_Pose_camRrect = ( right_camera_info . body_Pose_cam_ ) . compose ( camR_Pose_camRrect ) ; gtsam :: Pose3 camLrect_Pose_calRrect = B_Pose_camLrect -> between ( B_Pose_camRrect ) ; LOG_IF ( FATAL , gtsam :: Rot3 :: Logmap ( camLrect_Pose_calRrect . rotation ( ) ) . norm ( ) > 1e-5 ) << " " << gtsam :: Rot3 :: Logmap ( camL_Pose_camR . rotation ( ) ) . norm ( ) << ' \n ' << " " << gtsam :: Rot3 :: Logmap ( camLrect_Pose_calRrect . rotation ( ) ) . norm ( ) << ' \n ' << " " ; LOG_IF ( FATAL , fabs ( camLrect_Pose_calRrect . translation ( ) . y ( ) ) > 1e-3 || fabs ( camLrect_Pose_calRrect . translation ( ) . z ( ) ) > 1e-3 ) << " \n " << " " << camLrect_Pose_calRrect ; if ( left_camera_info . distortion_model_ == " " || left_camera_info . distortion_model_ == " " ) { VLOG ( 10 ) << " " ; cv :: initUndistortRectifyMap ( left_camera_info . K_ , left_camera_info . distortion_coeff_ , left_camera_info . R_rectify_ , P1 , left_camera_info . image_size_ , CV_32FC1 , left_camera_info . undistRect_map_x_ , left_camera_info . undistRect_map_y_ ) ; } else if ( left_camera_info . distortion_model_ == " " ) { VLOG ( 10 ) << " " ; cv :: fisheye :: initUndistortRectifyMap ( left_camera_info . K_ , left_camera_info . distortion_coeff_ , left_camera_info . R_rectify_ , P1 , left_camera_info . image_size_ , CV_32F , left_camera_info . undistRect_map_x_ , left_camera_info . undistRect_map_y_ ) ; } else { LOG ( ERROR ) << " " ; } if ( right_camera_info . distortion_model_ == " " || right_camera_info . distortion_model_ == " " ) { VLOG ( 10 ) << " " ; cv :: initUndistortRectifyMap ( right_camera_info . K_ , right_camera_info . distortion_coeff_ , right_camera_info . R_rectify_ , P2 , right_camera_info . image_size_ , CV_32FC1 , right_camera_info . undistRect_map_x_ , right_camera_info . undistRect_map_y_ ) ; } else if ( right_camera_info . distortion_model_ == " " ) { VLOG ( 10 ) << " " ; cv :: fisheye :: initUndistortRectifyMap ( right_camera_info . K_ , right_camera_info . distortion_coeff_ , right_camera_info . R_rectify_ , P2 , right_camera_info . image_size_ , CV_32F , right_camera_info . undistRect_map_x_ , right_camera_info . undistRect_map_y_ ) ; } else { LOG ( ERROR ) << " " ; } left_camera_info . P_ = P1 ; right_camera_info . P_ = P2 ; VLOG ( 10 ) << " " ; }
note also computes the rectification maps
TODO(Toni): this should be done much earlier and only once...
note also computes the rectification maps TODO ( Toni ) : this should be done much earlier and only once ...
// Get extrinsics in open CV format. //! Extrinsics of the stereo (not rectified) relative pose between cameras // NOTE: openCV pose convention is the opposite, that's why we have to invert ////////////////////////////////////////////////////////////////////////////// // get rectification matrices // P1 and P2 are the new camera matrices, but with an extra 0 0 0 column // Get stereo rectification // Input // Output // Get stereo rectification // following are output // TODO: Flag to maximise area??? // Left camera pose after rectification //! Fix camL position to camLrect. //! aka use gtsam::Point3() // right camera pose after rectification // Relative pose after rectification // Sanity check. ////////////////////////////////////////////////////////////////////////////// // TODO: Unit tests for this sections!!!!! // Left camera // Get rectification & undistortion maps. (radtan dist. model) // output: // Get rectification & undistortion maps. (equi dist. model) // output: // Right camera // Get rectification & undistortion maps. (radtan dist. model) // Output: // Get rectification & undistortion maps. (equi dist. model) // Output: // Store intermediate results from rectification. // contains an extra column to project in homogeneous coordinates // contains an extra column to project in homogeneous coordinates