From 273bbf8a9434bff1d617380d760bf4b56d8cb0ce Mon Sep 17 00:00:00 2001
From: Raphael Maenle <raphael@maenle.net>
Date: Tue, 25 Jun 2019 19:49:04 +0200
Subject: [PATCH] added fov camera model

---
 src/image_handler.cpp | 73 ++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 68 insertions(+), 5 deletions(-)

diff --git a/src/image_handler.cpp b/src/image_handler.cpp
index cb98426..d5e62ec 100644
--- a/src/image_handler.cpp
+++ b/src/image_handler.cpp
@@ -45,7 +45,22 @@ void undistortPoint(
   } else if (distortion_model == "equidistant") {
     cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                                  rectification_matrix, K_new);
-  } else {
+  }
+  else if (distortion_model == "fov") {
+    for(int i = 0; i < pts_in.size(); i++)
+    {
+      float omega = distortion_coeffs[0];
+      float rd = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
+      float ru = tan(rd * omega)/(2 * tan(omega / 2));
+
+      cv::Point2f newPoint(
+              ((pts_in[i].x - intrinsics[2]) / intrinsics[0]) * (ru / rd),
+              ((pts_in[i].y - intrinsics[3]) / intrinsics[1]) * (ru / rd));
+
+      pts_out.push_back(newPoint);
+    }
+   } 
+   else {
     ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...",
                   distortion_model.c_str());
     cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
@@ -79,10 +94,26 @@ void undistortPoints(
   if (distortion_model == "radtan") {
     cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                         rectification_matrix, K_new);
-  } else if (distortion_model == "equidistant") {
+  } 
+  else if (distortion_model == "equidistant") {
     cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                                  rectification_matrix, K_new);
-  } else {
+  } 
+  else if (distortion_model == "fov") {
+    for(int i = 0; i < pts_in.size(); i++)
+    {
+      float omega = distortion_coeffs[0];
+      float rd = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
+      float ru = tan(rd * omega)/(2 * tan(omega / 2));
+
+      cv::Point2f newPoint(
+              ((pts_in[i].x - intrinsics[2]) / intrinsics[0]) * (ru / rd),
+              ((pts_in[i].y - intrinsics[3]) / intrinsics[1]) * (ru / rd));
+
+      pts_out.push_back(newPoint);
+    }
+  } 
+  else {
     ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...",
                   distortion_model.c_str());
     cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
@@ -110,7 +141,23 @@ std::vector<cv::Point2f> distortPoints(
                       distortion_coeffs, pts_out);
   } else if (distortion_model == "equidistant") {
     cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
-  } else {
+  }
+  else if (distortion_model == "fov") {
+    for(int i = 0; i < pts_in.size(); i++)
+    {
+      // based on 'straight lines have to be straight'
+      float ru = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
+      float omega = distortion_coeffs[0];
+      float rd = 1 / (omega)*atan(2*ru*tan(omega / 2));
+      
+      cv::Point2f newPoint(
+              pts_in[i].x * (rd/ru) * intrinsics[0] + intrinsics[2],
+              pts_in[i].y * (rd/ru) * intrinsics[1] + intrinsics[3]);
+
+      pts_out.push_back(newPoint);
+    }
+  } 
+   else {
     ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
                   distortion_model.c_str());
     std::vector<cv::Point3f> homogenous_pts;
@@ -143,7 +190,23 @@ cv::Point2f distortPoint(
                       distortion_coeffs, pts_out);
   } else if (distortion_model == "equidistant") {
     cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
-  } else {
+  }
+  else if (distortion_model == "fov") {
+    for(int i = 0; i < pts_in.size(); i++)
+    {
+      // based on 'straight lines have to be straight'
+      float ru = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
+      float omega = distortion_coeffs[0];
+      float rd = 1 / (omega)*atan(2*ru*tan(omega / 2));
+      
+      cv::Point2f newPoint(
+              pts_in[i].x * (rd/ru) * intrinsics[0] + intrinsics[2],
+              pts_in[i].y * (rd/ru) * intrinsics[1] + intrinsics[3]);
+
+      pts_out.push_back(newPoint);
+    }
+  } 
+   else {
     ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
                   distortion_model.c_str());
     std::vector<cv::Point3f> homogenous_pts;