[Feat] add fisheye undistortion to EquidistantCamera

README.md

@@ -1,5 +1,31 @@
-VIKIT
------
+## Vikit for FAST-LIVO2
 
-Vikit (Vision-Kit) provides some tools for your vision/robotics project.
-Far from stable.
+Vikit (Vision-Kit) is a lightweight and efficient library providing essential camera models, mathematical utilities, and interpolation functions for the FAST-LIVO2 framework. 
+
+📬 For further assistance or inquiries, please contact Chunran Zheng at [email protected].
+
+### Supported Camera Models
+Vikit supports the following camera models:
+
+- Pinhole + Radtan
+- Pinhole + Equidistant **(with OpenCV-based fisheye distortion correction)**
+- Pinhole + ATAN
+- Pinhole + Polynomial
+- Omni + Polynomial
+
+These models accommodate diverse camera configurations and distortion characteristics.
+
+### Project Lineage
+This repository is maintained as a downstream fork with the following lineage:
+1. Original work: [xuankuzcr/rpg_vikit](https://github.com/xuankuzcr/rpg_vikit) by Chunran Zheng.
+2. ROS2-compatible port: [integralrobotics/rpg_vikit](https://github.com/integralrobotics/rpg_vikit).
+
+### Enhancements
+The following key improvements have been implemented:
+- Integrated OpenCV-based fisheye distortion correction into the equidistant camera model (`vikit_common` module).
+
+### Acknowledgments
+Special recognition to:
+- [UZH-RPG group](https://rpg.ifi.uzh.ch/) for foundational [rpg_vikit](https://github.com/uzh-rpg/rpg_vikit) research.
+- [Chunran Zheng](https://github.com/xuankuzcr/rpg_vikit) for developing the ROS1-compatible version [xuankuzcr/rpg_vikit](https://github.com/xuankuzcr/rpg_vikit) specifically adapted for FAST-LIVO2.
+- [integralrobotics](https://github.com/integralrobotics) for porting Chunran Zheng's version to ROS2 in their [rpg_vikit implementation](https://github.com/integralrobotics/rpg_vikit).

vikit_common/include/vikit/equidistant_camera.h

@@ -137,6 +137,9 @@ class EquidistantCamera : public AbstractCamera {
   virtual double fy() const { return fy_; };
   virtual double cx() const { return cx_; };
   virtual double cy() const { return cy_; };
+
+  // Undistort the input image to correct lens distortion
+  void undistortImage(const cv::Mat& input, cv::Mat& output) const;
 };
 
 } // end namespace vk

vikit_common/src/equidistant_camera.cpp

@@ -108,4 +108,25 @@ world2cam(const Vector2d& uv) const
   return px;
 }
 
+void EquidistantCamera::undistortImage(const cv::Mat& input, cv::Mat& output) const
+{
+  // Return original image if no distortion parameters are available
+  if(!distortion_) {
+    output = input.clone();
+    return;
+  }
+  // Build camera intrinsic matrix K
+  cv::Mat K = cv::Mat::eye(3, 3, CV_64F);
+  K.at<double>(0,0) = fx_;  // Focal length in x direction
+  K.at<double>(1,1) = fy_;  // Focal length in y direction
+  K.at<double>(0,2) = cx_;  // Principal point x-coordinate
+  K.at<double>(1,2) = cy_;  // Principal point y-coordinate
+  // Prepare distortion coefficients (k1, k2, k3, k4)
+  cv::Mat D = (cv::Mat_<double>(4,1) << k1_, k2_, k3_, k4_);
+  // Create new camera matrix (using same intrinsics by default)
+  cv::Mat new_K = K.clone();
+  // Apply fisheye lens undistortion using OpenCV
+  cv::fisheye::undistortImage(input, output, K, D, new_K);
+}
+
 } // end namespace vk