Eigen::Matrix2Xd HexGridFitting::reprojectDots()

in IsometricPatternMatcher/HexGridFitting.cpp [429:455]

• 27 lines of code
• 3 McCabe index (conditional complexity)

Eigen::Matrix2Xd HexGridFitting::reprojectDots(
    const Sophus::SE3d& T_camera_target,
    const Eigen::Vector4d& distortionParams,
    const Eigen::Matrix2Xd& imageDots) {
  Eigen::Matrix2Xd result;
  result.resize(2, imageDots.cols());
  Sophus::Plane3d plane(Sophus::Vector3d(0.0, 0.0, 1.0), 0);
  for (int i = 0; i < imageDots.cols(); ++i) {
    Ray3d rayInCamera;
    if (ifDistort_) {
      Sophus::Vector<double, KannalaBrandtK3Projection::kNumParams> intrinsics;
      intrinsics << focalLength_, focalLength_, centerXY_(0), centerXY_(1),
          distortionParams(0), distortionParams(1), distortionParams(2),
          distortionParams(3);
      rayInCamera =
          KannalaBrandtK3Projection::unproject(imageDots.col(i), intrinsics);
    } else {
      Sophus::Vector<double, PinholeProjection::kNumParams> intrinsics;
      intrinsics << focalLength_, focalLength_, centerXY_(0), centerXY_(1);
      rayInCamera = PinholeProjection::unproject(imageDots.col(i), intrinsics);
    }
    Ray3d rayInTarget = T_camera_target.inverse() * rayInCamera;
    Sophus::Vector3d ptTarget3d = rayInTarget.line().intersectionPoint(plane);
    result.col(i) = ptTarget3d.head<2>();
  }
  return result;
}