camera.hpp

// Copyright (C) 2023 Alessandro Fornasier.
// Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the Apache License, Version 2.0
// (the "License"); you may not use this file except in compliance with the
// License. You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations
// under the License.
//
// You can contact the authors at <alessandro.fornasier@ieee.org>
 
#ifndef CAMERA_HPP
#define CAMERA_HPP
 
#include <memory>
#include <opencv2/opencv.hpp>
 
#include "msceqf/options/msceqf_options.hpp"
#include "types/fptypes.hpp"
 
namespace msceqf
{
class PinholeCamera
{
 public:
  virtual ~PinholeCamera() = default;

  void undistort(std::vector<Eigen::Vector2f>& uv, const bool& normalize = false);

  virtual void undistort(std::vector<cv::Point2f>& uv_cv, const bool& normalize = false) = 0;

  virtual void undistortImage(const cv::Mat& image, cv::Mat& image_undistorted) = 0;

  void normalize(std::vector<Eigen::Vector2f>& uv);

  void normalize(std::vector<cv::Point2f>& uv);

  void normalize(Eigen::Vector2f& uv);

  void normalize(cv::Point2f& uv);

  void denormalize(std::vector<Eigen::Vector2f>& uv);

  void denormalize(std::vector<cv::Point2f>& uv);

  void denormalize(Eigen::Vector2f& uv);

  void denormalize(cv::Point2f& uv);

  void setIntrinsics(const Vector4& intrinsics);

  const Vector4& intrinsics() const;

  const VectorX& distortionCoefficients() const;
 
 protected:
  PinholeCamera(const VectorX& distortion_coefficients,
                const Vector4 instrinsics,
                const uint& width,
                const uint& height);

  PinholeCamera() = delete;
  PinholeCamera(const PinholeCamera&) = delete;
  PinholeCamera(PinholeCamera&&) = delete;
  PinholeCamera& operator=(const PinholeCamera&) = delete;
  PinholeCamera& operator=(PinholeCamera&&) = delete;
 
  VectorX distortion_coefficients_;  
  Vector4 intrinsics_;               
  uint width_;                       
  uint height_;                      
};

struct RadtanCamera final : public PinholeCamera
{
  RadtanCamera(const CameraOptions& opts, const Vector4& intrinsics);

  void undistort(std::vector<cv::Point2f>& uv_cv, const bool& normalize) override;

  void undistortImage(const cv::Mat& image, cv::Mat& image_undistorted) override;
};
 
struct EquidistantCamera final : public PinholeCamera
{
  EquidistantCamera(const CameraOptions& opts, const Vector4& intrinsics);

  void undistort(std::vector<cv::Point2f>& uv_cv, const bool& normalize) override;

  void undistortImage(const cv::Mat& image, cv::Mat& image_undistorted) override;
};
 
using PinholeCameraSharedPtr = std::shared_ptr<PinholeCamera>;
using PinholeCameraUniquePtr = std::unique_ptr<PinholeCamera>;
using RadtanCameraSharedPtr = std::shared_ptr<RadtanCamera>;
using RadtanCameraUniquePtr = std::unique_ptr<RadtanCamera>;
using EquidistantCameraSharedPtr = std::shared_ptr<EquidistantCamera>;
using EquidistantCameraUniquePtr = std::unique_ptr<EquidistantCamera>;

template <typename T>
[[nodiscard]] static PinholeCameraUniquePtr createCamera(const CameraOptions& opts, const Vector4& intrinsics)
{
  if constexpr (std::is_base_of_v<PinholeCamera, T>)
  {
    return std::make_unique<T>(opts, intrinsics);
  }
  else
  {
    return nullptr;
  }
}
 
}  // namespace msceqf
 
#endif  // CAMERA_HPP