equidistant_camera.cpp

/*
Equidistant (fisheye, KB4) camera model.
*/

#include <vikit/equidistant_camera.h>
#include <opencv2/calib3d/calib3d.hpp>

namespace vk
{

EquidistantCamera::EquidistantCamera(double width, double height, double scale, double fx, double fy, double cx, double cy, double k1, double k2,
                                     double k3, double k4)
    : AbstractCamera(static_cast<int>(width * scale), static_cast<int>(height * scale)), fx_(fx * scale), fy_(fy * scale), cx_(cx * scale),
      cy_(cy * scale), distortion_(fabs(k1) > 1e-8 || fabs(k2) > 1e-8 || fabs(k3) > 1e-8 || fabs(k4) > 1e-8)
{
  scale_ = scale;
  k_[0] = k1;
  k_[1] = k2;
  k_[2] = k3;
  k_[3] = k4;
  cvK_ = (cv::Mat_<double>(3, 3) << fx_, 0.0, cx_, 0.0, fy_, cy_, 0.0, 0.0, 1.0);
  cvD_ = (cv::Mat_<double>(4, 1) << k_[0], k_[1], k_[2], k_[3]);
  cv::fisheye::initUndistortRectifyMap(cvK_, cvD_, cv::Mat_<double>::eye(3, 3), cvK_, cv::Size(width_, height_), CV_16SC2, undist_map1_,
                                       undist_map2_);
}

EquidistantCamera::~EquidistantCamera() {}

Vector3d EquidistantCamera::cam2world(const double &u, const double &v) const
{
  Vector3d xyz;
  if (!distortion_)
  {
    xyz[0] = (u - cx_) / fx_;
    xyz[1] = (v - cy_) / fy_;
    xyz[2] = 1.0;
  }
  else
  {
    cv::Point2d uv(u, v), px;
    const cv::Mat src_pt(1, 1, CV_64FC2, &uv.x);
    cv::Mat dst_pt(1, 1, CV_64FC2, &px.x);
    cv::fisheye::undistortPoints(src_pt, dst_pt, cvK_, cvD_);
    xyz[0] = px.x;
    xyz[1] = px.y;
    xyz[2] = 1.0;
  }
  return xyz.normalized();
}

Vector3d EquidistantCamera::cam2world(const Vector2d &uv) const { return cam2world(uv[0], uv[1]); }

Vector2d EquidistantCamera::world2cam(const Vector3d &xyz) const { return world2cam(Vector2d(xyz[0] / xyz[2], xyz[1] / xyz[2])); }

Vector2d EquidistantCamera::world2cam(const Vector2d &uv) const
{
  Vector2d px;
  const double x = uv[0], y = uv[1];
  const double r = std::sqrt(x * x + y * y);
  if (!distortion_ || r < 1e-9)
  {
    px[0] = fx_ * x + cx_;
    px[1] = fy_ * y + cy_;
    return px;
  }
  const double theta = std::atan(r);
  const double theta2 = theta * theta, theta4 = theta2 * theta2, theta6 = theta4 * theta2, theta8 = theta4 * theta4;
  const double theta_d = theta * (1.0 + k_[0] * theta2 + k_[1] * theta4 + k_[2] * theta6 + k_[3] * theta8);
  const double s = theta_d / r;
  px[0] = fx_ * x * s + cx_;
  px[1] = fy_ * y * s + cy_;
  return px;
}

void EquidistantCamera::undistortImage(const cv::Mat &raw, cv::Mat &rectified) const
{
  if (distortion_)
    cv::remap(raw, rectified, undist_map1_, undist_map2_, cv::INTER_LINEAR);
  else
    rectified = raw.clone();
}

} // namespace vk