[Feat] add fisheye undistortion to EquidistantCamera

[Rhymer-Lcy]

New Feature

Added real-time fisheye undistortion method to EquidistantCamera class:

void EquidistantCamera::undistortImage(const cv::Mat& input, cv::Mat& output) const {
  if(!distortion_) { output = input.clone(); return; }  // Skip if no distortion
  cv::Mat K = (cv::Mat_<double>(3,3) << fx_,0,cx_,0,fy_,cy_,0,0,1); // Intrinsic matrix
  cv::Mat D = (cv::Mat_<double>(4,1) << k1_,k2_,k3_,k4_);           // Distortion coeffs
  cv::fisheye::undistortImage(input, output, K, D, K);              // OpenCV correction
}