Different camera models

CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 2.18...3.16)
 
-set(VERSION 1.2.3)
+set(VERSION 1.2.4)
 project(x VERSION ${VERSION} LANGUAGES C CXX)
 
 if (NOT CMAKE_BUILD_TYPE)
@@ -213,6 +213,11 @@ include_directories(include
         )
 
 set(SOURCE
+        src/x/camera_models/camera_model.cpp
+        src/x/camera_models/camera_fov.cpp
+        src/x/camera_models/camera_radtan.cpp
+        src/x/camera_models/camera_equidistant.cpp
+        src/x/camera_models/camera_none.cpp
         src/x/ekf/ekf.cpp
         src/x/ekf/propagator.cpp
         src/x/ekf/state.cpp
@@ -227,7 +232,6 @@ set(SOURCE
         src/x/vio/slam_update.cpp
         src/x/vio/range_update.cpp
         src/x/vio/solar_update.cpp
-        src/x/vision/camera.cpp
         src/x/vision/feature.cpp
         src/x/vision/tiled_image.cpp
         src/x/vision/tracker.cpp

README.md

@@ -134,7 +134,7 @@ $ git clone git@github.com:jpl-x/x_multi_agent.git
 $ mkdir build && cd build
 $ cmake ..
 $ make package
-$ sudo dpkg -i x_1.2.3_$(dpkg --print-architecture).deb
+$ sudo dpkg -i x_1.2.4_$(dpkg --print-architecture).deb
 ```
 
 To enable/disable some features of the collaborative approach, set to ON/OFF the CMake options:

include/x/camera_models/camera.h

@@ -0,0 +1,29 @@
+//
+// Created by viciopoli on 05.01.23.
+//
+
+#ifndef X_CAMERA_H
+#define X_CAMERA_H
+
+#include "x/camera_models/camera_model.h"
+#include "x/camera_models/camera_equidistant.h"
+#include "x/camera_models/camera_radtan.h"
+#include "x/camera_models/camera_fov.h"
+#include "x/camera_models/camera_none.h"
+
+namespace x::Camera {
+    static std::shared_ptr<CameraModel> constructCamera(const Params &params) {
+        // Initialize camera geometry
+        if (params.cameraType == Camera::DistortionModel::RADTAN) {
+            return std::make_shared<CameraRadTan>(params);
+        } else if (params.cameraType == Camera::DistortionModel::EQUIDISTANT) {
+            return std::make_shared<CameraEquidistant>(params);
+        } else if (params.cameraType == Camera::DistortionModel::FOV) {
+            return std::make_shared<CameraFov>(params);
+        } else {
+            return std::make_shared<CameraNone>(params);
+        }
+    }
+}
+
+#endif //X_CAMERA_H

include/x/camera_models/camera_equidistant.h

@@ -0,0 +1,24 @@
+//
+// Created by viciopoli on 09.10.22.
+//
+
+#ifndef X_CAMERA_EQUIDISTANT_H
+#define X_CAMERA_EQUIDISTANT_H
+
+#include "camera_model.h"
+
+namespace x {
+    class CameraEquidistant : public CameraModel {
+    public:
+        explicit CameraEquidistant(const Camera::Params &params) : CameraModel(params) {}
+
+        ~CameraEquidistant() override = default;
+
+        void undistort(Feature &feature) override;
+
+    private:
+    };
+} // x
+
+
+#endif //X_CAMERA_EQUIDISTANT_H

include/x/camera_models/camera_fov.h

@@ -0,0 +1,29 @@
+//
+// Created by viciopoli on 09.10.22.
+//
+
+#ifndef X_CAMERA_FOV_H
+#define X_CAMERA_FOV_H
+
+#include "camera_model.h"
+
+namespace x {
+    class CameraFov : public CameraModel {
+    public:
+        CameraFov(const Camera::Params &params) : CameraModel(params) {
+            s_term_ = 1.0 / (2.0 * std::tan(params.dist_coeff[0] / 2.0));
+        }
+
+        ~CameraFov() override = default;
+
+        void undistort(Feature &feature) override;
+
+    private:
+        double s_term_;
+
+        // Inverse FOV distortion transformation
+        [[nodiscard]] double inverseTf(double dist) const;
+    };
+} // x
+
+#endif //X_CAMERA_FOV_H

include/x/camera_models/camera_model.h

@@ -0,0 +1,67 @@
+//
+// Created by viciopoli on 09.10.22.
+//
+
+#ifndef X_CAMERAMODEL_H
+#define X_CAMERAMODEL_H
+
+#include "x/vision/feature.h"
+#include "x/vision/types.h"
+#include <utility>
+#include <vector>
+#include <optional>
+#include <opencv2/core.hpp>
+#include <opencv2/calib3d.hpp>
+#include "x/camera_models/types.h"
+
+namespace x {
+    /**
+     * Base class
+     */
+    class CameraModel {
+    public:
+
+        explicit CameraModel(const Camera::Params &params);
+
+        virtual ~CameraModel() = default;
+
+        void set(const Camera::Params &cameraParams) {
+            cameraParams_ = cameraParams;
+        };
+
+        void undistort(FeatureList &features);
+
+        virtual void undistort(Feature &feature) = 0;
+
+        [[nodiscard]] unsigned int getWidth() const { return cameraParams_.img_width_; };
+
+        [[nodiscard]] unsigned int getHeight() const { return cameraParams_.img_height_; };
+
+        [[nodiscard]] double getFx() const { return cameraParams_.fx_; };
+
+        [[nodiscard]] double getFy() const { return cameraParams_.fy_; };
+
+        [[nodiscard]] Feature normalize(const Feature &feature) const;
+
+        [[nodiscard]] Track normalize(const Track &track, size_t max_size = 0) const;
+
+        [[nodiscard]] TrackList normalize(const TrackList &tracks, size_t max_size = 0) const;
+
+        [[nodiscard]] Matrix getCameraMatrix() const;
+
+        [[nodiscard]] cv::Matx33d getCVCameraMatrix() const;
+
+#ifdef MULTI_UAV
+        [[nodiscard]] cv::KeyPoint normalize(const cv::KeyPoint &point) const;
+        [[nodiscard]] cv::KeyPoint denormalize(const cv::KeyPoint &point) const;
+#endif
+
+    protected:
+        Camera::Params cameraParams_;
+
+    private:
+
+    };
+} // x
+
+#endif //X_CAMERAMODEL_H

include/x/camera_models/camera_none.h

@@ -0,0 +1,23 @@
+//
+// Created by viciopoli on 09.10.22.
+//
+
+#ifndef X_CAMERA_NONE_H
+#define X_CAMERA_NONE_H
+
+#include "camera_model.h"
+
+namespace x {
+    class CameraNone : public CameraModel {
+    public:
+        explicit CameraNone(const Camera::Params &params) : CameraModel(params) {}
+
+        ~CameraNone() override = default;
+
+        void undistort(Feature &feature) override;
+
+    private:
+    };
+} // x
+
+#endif //X_CAMERA_NONE_H

include/x/camera_models/camera_radtan.h

@@ -0,0 +1,23 @@
+//
+// Created by viciopoli on 09.10.22.
+//
+
+#ifndef X_CAMERA_RAD_TAN_H
+#define X_CAMERA_RAD_TAN_H
+
+#include "camera_model.h"
+
+namespace x {
+    class CameraRadTan : public CameraModel {
+    public:
+        explicit CameraRadTan(const Camera::Params &params) : CameraModel(params) {}
+
+        ~CameraRadTan() override = default;
+
+        void undistort(Feature &feature) override;
+
+    private:
+    };
+} // x
+
+#endif //X_CAMERA_RAD_TAN_H

include/x/camera_models/types.h

@@ -0,0 +1,79 @@
+//
+// Created by viciopoli on 05.01.23.
+//
+
+#ifndef X_TYPES_H
+#define X_TYPES_H
+
+namespace x::Camera {
+
+    enum DistortionModel {
+        FOV, RADTAN, EQUIDISTANT, NONE
+    };
+
+    struct Params {
+        unsigned int img_width_;
+        unsigned int img_height_;
+
+        double fx_;
+        double fy_;
+        double cx_;
+        double cy_;
+
+        std::vector<double> dist_coeff;
+
+        double inv_fx_;
+        double inv_fy_;
+        double cx_n_;
+        double cy_n_;
+
+        DistortionModel cameraType;
+
+        Params(double fx,
+               double fy,
+               double cx,
+               double cy,
+               std::vector<double> dist_coeff,
+               unsigned int img_width,
+               unsigned int img_height,
+               std::string distortion_model) :
+                img_width_(img_width),
+                img_height_(img_height),
+                fx_(fx * img_width), fy_(fy * img_height),
+                cx_(cx * img_width), cy_(cy * img_height),
+                dist_coeff(std::move(dist_coeff)),
+                inv_fx_(1.0 / fx_),
+                inv_fy_(1.0 / fy_), cx_n_(cx_ * inv_fx_), cy_n_(cy_ * inv_fy_) {
+            cameraType = distortionString2Enum(distortion_model);
+        };
+
+        DistortionModel distortionString2Enum(const std::string &distortion_model) {
+            if (distortion_model == "FOV") {
+                return DistortionModel::FOV;
+            } else if (distortion_model == "RADTAN") {
+                return DistortionModel::RADTAN;
+            } else if (distortion_model == "EQUIDISTANT") {
+                return DistortionModel::EQUIDISTANT;
+            } else {
+                return DistortionModel::NONE;
+            }
+        }
+
+
+        [[nodiscard]] cv::Matx33d getCameraMatrix() const {
+            static cv::Matx33d m(fx_, 0.0, cx_,
+                                 0.0, fy_, cy_,
+                                 0.0, 0.0, 1.0);
+            return m;
+        }
+
+        [[nodiscard]] Matrix3 getCameraMatrixEigen() const {
+            static Matrix3 m;
+            m << fx_, 0.0, cx_,
+                    0.0, fy_, cy_,
+                    0.0, 0.0, 1.0;
+            return m;
+        }
+    };
+}
+#endif //X_TYPES_H

include/x/vio/track_manager.h

@@ -18,9 +18,9 @@
 #define TRACK_MANAGER_H_
 
 #include <opencv2/imgproc/imgproc.hpp>
-#include <x/vio/types.h>
-#include <x/vision/camera.h>
-#include <x/vision/types.h>
+#include "x/vio/types.h"
+#include "x/camera_models/camera.h"
+#include "x/vision/types.h"
 
 namespace x {
 /**
@@ -33,9 +33,11 @@ namespace x {
 class TrackManager {
 public:
   TrackManager();
-  TrackManager(const Camera &camera, const double min_baseline_x_n,
+
+  TrackManager(std::shared_ptr<CameraModel> camera, const double min_baseline_x_n,
                const double min_baseline_y_n);
-  void setCamera(Camera camera);
+
+  void setCamera(std::shared_ptr<CameraModel> camera);
 
   // All track getters are in normalized image coordinates
   TrackList getMsckfTracks() const; // Tracks ready for MSCKF update
@@ -97,7 +99,7 @@ class TrackManager {
   TrackList getMostPromisingTracks(int k);
 
 private:
-  Camera camera_;
+    std::shared_ptr<CameraModel> camera_;
   TrackList slam_trks_; // persistent feature tracks (excluding new ones)
   // New SLAM tracks (both semi-infinite depths uncertainty and MSCKF)
   TrackList new_slam_trks_;

include/x/vio/types.h

@@ -23,6 +23,7 @@
 #include "x/vision/feature.h"
 #include "x/vision/tiled_image.h"
 #include "x/vision/types.h"
+#include "x/camera_models/camera.h"
 /**
  * This header defines common types used in xVIO.
  */
@@ -47,13 +48,8 @@ struct Params {
   Vector3 sigma_dbw;
   Vector3 sigma_dba;
   // camera parameters
-  double cam_fx;
-  double cam_fy;
-  double cam_cx;
-  double cam_cy;
-  double cam_s;
-  int img_height;
-  int img_width;
+  std::shared_ptr<CameraModel> camera;
+
   // imu-camera calibration parameters
   Vector3 p_ic;
   Quaternion q_ic;
@@ -74,7 +70,7 @@ struct Params {
   double n_w;
   double n_bw;
 
-  // Tracking and detector paramters
+        // Tracking and detector parameters
   int fast_detection_delta;
   bool non_max_supp;
   int block_half_length;