From 2f4b792eb85930f1973d7631ff63e024f5a7f9d5 Mon Sep 17 00:00:00 2001 From: Vincenzo Polizzi Date: Fri, 30 Jun 2023 17:51:18 +0200 Subject: [PATCH] Different camera models --- CMakeLists.txt | 8 +- README.md | 2 +- include/x/camera_models/camera.h | 29 ++++ include/x/camera_models/camera_equidistant.h | 24 +++ include/x/camera_models/camera_fov.h | 29 ++++ include/x/camera_models/camera_model.h | 67 ++++++++ include/x/camera_models/camera_none.h | 23 +++ include/x/camera_models/camera_radtan.h | 23 +++ include/x/camera_models/types.h | 79 +++++++++ include/x/vio/track_manager.h | 14 +- include/x/vio/types.h | 12 +- include/x/vio/vio.h | 4 +- include/x/vision/camera.h | 74 -------- include/x/vision/tracker.h | 8 +- src/x/camera_models/camera_equidistant.cpp | 19 +++ src/x/camera_models/camera_fov.cpp | 35 ++++ src/x/camera_models/camera_model.cpp | 82 +++++++++ src/x/camera_models/camera_none.cpp | 12 ++ src/x/camera_models/camera_radtan.cpp | 19 +++ src/x/vio/track_manager.cpp | 30 ++-- src/x/vio/vio.cpp | 44 ++--- src/x/vision/camera.cpp | 169 ------------------- src/x/vision/tracker.cpp | 10 +- 23 files changed, 511 insertions(+), 305 deletions(-) create mode 100644 include/x/camera_models/camera.h create mode 100644 include/x/camera_models/camera_equidistant.h create mode 100644 include/x/camera_models/camera_fov.h create mode 100644 include/x/camera_models/camera_model.h create mode 100644 include/x/camera_models/camera_none.h create mode 100644 include/x/camera_models/camera_radtan.h create mode 100644 include/x/camera_models/types.h delete mode 100644 include/x/vision/camera.h create mode 100644 src/x/camera_models/camera_equidistant.cpp create mode 100644 src/x/camera_models/camera_fov.cpp create mode 100644 src/x/camera_models/camera_model.cpp create mode 100644 src/x/camera_models/camera_none.cpp create mode 100644 src/x/camera_models/camera_radtan.cpp delete mode 100644 src/x/vision/camera.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1556607..9d87d9b 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/README.md b/README.md index e9b8eef..25e1e9a 100644 --- a/README.md +++ b/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: diff --git a/include/x/camera_models/camera.h b/include/x/camera_models/camera.h new file mode 100644 index 0000000..86af4aa --- /dev/null +++ b/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 constructCamera(const Params ¶ms) { + // Initialize camera geometry + if (params.cameraType == Camera::DistortionModel::RADTAN) { + return std::make_shared(params); + } else if (params.cameraType == Camera::DistortionModel::EQUIDISTANT) { + return std::make_shared(params); + } else if (params.cameraType == Camera::DistortionModel::FOV) { + return std::make_shared(params); + } else { + return std::make_shared(params); + } + } +} + +#endif //X_CAMERA_H diff --git a/include/x/camera_models/camera_equidistant.h b/include/x/camera_models/camera_equidistant.h new file mode 100644 index 0000000..a020d0a --- /dev/null +++ b/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 ¶ms) : CameraModel(params) {} + + ~CameraEquidistant() override = default; + + void undistort(Feature &feature) override; + + private: + }; +} // x + + +#endif //X_CAMERA_EQUIDISTANT_H diff --git a/include/x/camera_models/camera_fov.h b/include/x/camera_models/camera_fov.h new file mode 100644 index 0000000..8cccba0 --- /dev/null +++ b/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 ¶ms) : 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 diff --git a/include/x/camera_models/camera_model.h b/include/x/camera_models/camera_model.h new file mode 100644 index 0000000..0f4ed6a --- /dev/null +++ b/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 +#include +#include +#include +#include +#include "x/camera_models/types.h" + +namespace x { + /** + * Base class + */ + class CameraModel { + public: + + explicit CameraModel(const Camera::Params ¶ms); + + 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 diff --git a/include/x/camera_models/camera_none.h b/include/x/camera_models/camera_none.h new file mode 100644 index 0000000..457b602 --- /dev/null +++ b/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 ¶ms) : CameraModel(params) {} + + ~CameraNone() override = default; + + void undistort(Feature &feature) override; + + private: + }; +} // x + +#endif //X_CAMERA_NONE_H diff --git a/include/x/camera_models/camera_radtan.h b/include/x/camera_models/camera_radtan.h new file mode 100644 index 0000000..2757f41 --- /dev/null +++ b/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 ¶ms) : CameraModel(params) {} + + ~CameraRadTan() override = default; + + void undistort(Feature &feature) override; + + private: + }; +} // x + +#endif //X_CAMERA_RAD_TAN_H diff --git a/include/x/camera_models/types.h b/include/x/camera_models/types.h new file mode 100644 index 0000000..e91b5e4 --- /dev/null +++ b/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 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 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 diff --git a/include/x/vio/track_manager.h b/include/x/vio/track_manager.h index 542ea8e..6abdca0 100644 --- a/include/x/vio/track_manager.h +++ b/include/x/vio/track_manager.h @@ -18,9 +18,9 @@ #define TRACK_MANAGER_H_ #include -#include -#include -#include +#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 camera, const double min_baseline_x_n, const double min_baseline_y_n); - void setCamera(Camera camera); + + void setCamera(std::shared_ptr 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 camera_; TrackList slam_trks_; // persistent feature tracks (excluding new ones) // New SLAM tracks (both semi-infinite depths uncertainty and MSCKF) TrackList new_slam_trks_; diff --git a/include/x/vio/types.h b/include/x/vio/types.h index 37dbeea..8173cde 100644 --- a/include/x/vio/types.h +++ b/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 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; diff --git a/include/x/vio/vio.h b/include/x/vio/vio.h index eaf7d44..dd305a0 100644 --- a/include/x/vio/vio.h +++ b/include/x/vio/vio.h @@ -32,7 +32,7 @@ #include "x/vio/track_manager.h" #include "x/vio/types.h" #include "x/vio/vio_updater.h" -#include "x/vision/camera.h" +#include "x/camera_models/camera.h" #include "x/vision/tiled_image.h" #include "x/vision/tracker.h" @@ -240,7 +240,7 @@ class VIO { */ double msckf_baseline_x_n_, msckf_baseline_y_n_; - Camera camera_; + std::shared_ptr camera_; Tracker tracker_; TrackManager track_manager_; StateManager state_manager_; diff --git a/include/x/vision/camera.h b/include/x/vision/camera.h deleted file mode 100644 index ecc2062..0000000 --- a/include/x/vision/camera.h +++ /dev/null @@ -1,74 +0,0 @@ -/* - * Copyright 2020 California Institute of Technology (“Caltech”) - * - * Licensed under 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. - */ - -#ifndef JPL_VPP_CAMERA_H_ -#define JPL_VPP_CAMERA_H_ - -#include - -#include "x/vision/track.h" -#include "x/vision/types.h" - -namespace x { -class Camera { - public: - Camera(); - Camera(double fx, double fy, double cx, double cy, double s, - unsigned int img_width, unsigned int img_height); - [[nodiscard]] unsigned int getWidth() const; - [[nodiscard]] unsigned int getHeight() const; - [[nodiscard]] double getInvFx() const; - [[nodiscard]] double getInvFy() const; - [[nodiscard]] double getCxN() const; - [[nodiscard]] double getCyN() const; - void undistort(FeatureList &features) const; - void undistort(Feature &feature) const; -// Returns image coordinates in normal plane -#ifdef MULTI_UAV - [[nodiscard]] cv::KeyPoint normalize(const cv::KeyPoint &point) const; - [[nodiscard]] cv::KeyPoint denormalize(const cv::KeyPoint &point) const; -#endif - [[nodiscard]] Matrix getCameraMatrix() const; - [[nodiscard]] cv::Mat getCVCameraMatrix() const; - [[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; - - private: - double fx_ = 0.0; // Focal length - double fy_ = 0.0; - double cx_ = 0.0; // Principal point - double cy_ = 0.0; - double s_ = 0.0; // Distortion - unsigned int img_width_ = 0.0; - unsigned int img_height_ = 0.0; - // Distortion terms we only want to compute once - double inv_fx_ = -1; // Inverse focal length - double inv_fy_ = -1; - double cx_n_ = -1; // Principal point in normalized coordinates - double cy_n_ = -1; - double s_term_ = -1; // Distortion term - Matrix K_; - cv::Mat cvK_; - // Inverse FOV distortion transformation - [[nodiscard]] double inverseTf(const double& dist) const; - - -}; -} // namespace x - -#endif diff --git a/include/x/vision/tracker.h b/include/x/vision/tracker.h index 134f881..d51907d 100644 --- a/include/x/vision/tracker.h +++ b/include/x/vision/tracker.h @@ -25,7 +25,7 @@ #include #include -#include "x/vision/camera.h" +#include "x/camera_models/camera.h" #include "x/vision/types.h" #ifdef MULTI_UAV @@ -72,7 +72,7 @@ class Tracker { * @param win_size_h_photo * @param fast_detection_delta_photo */ - Tracker(const Camera &cam, int fast_detection_delta, bool non_max_supp, + Tracker(const std::shared_ptr cam, int fast_detection_delta, bool non_max_supp, unsigned int block_half_length, unsigned int margin, unsigned int n_feat_min, int outlier_method, double outlier_param1, double outlier_param2, int win_size_w, int win_size_h, int max_level, @@ -118,7 +118,7 @@ class Tracker { * @param win_size_h_photo * @param fast_detection_delta_photo */ - void setParams(const Camera &cam, int fast_detection_delta, bool non_max_supp, + void setParams(std::shared_ptr cam, int fast_detection_delta, bool non_max_supp, unsigned int block_half_length, unsigned int margin, unsigned int n_feat_min, int outlier_method, double outlier_param1, double outlier_param2, int win_size_w, @@ -241,7 +241,7 @@ class Tracker { double previous_timestamp_{0}; TiledImage previous_img_; // Tracker params - Camera camera_; + std::shared_ptr camera_; int fast_detection_delta_ = 9; // the intensity difference threshold for the FAST feature detector bool non_max_supp_ = true; // if true, non-maximum suppression is applied to diff --git a/src/x/camera_models/camera_equidistant.cpp b/src/x/camera_models/camera_equidistant.cpp new file mode 100644 index 0000000..230d76b --- /dev/null +++ b/src/x/camera_models/camera_equidistant.cpp @@ -0,0 +1,19 @@ +// +// Created by viciopoli on 09.10.22. +// + +#include "x/camera_models/camera_equidistant.h" + +using namespace x; + +void CameraEquidistant::undistort(Feature &feature) { + + const std::vector src{feature.getDistPoint2f()}; + std::vector res; + cv::fisheye::undistortPoints(src, res, cameraParams_.getCameraMatrix(), cameraParams_.dist_coeff); + + feature.setX(res[0].x * cameraParams_.fx_ + cameraParams_.cx_); + feature.setY(res[0].y * cameraParams_.fy_ + cameraParams_.cy_); +} + + diff --git a/src/x/camera_models/camera_fov.cpp b/src/x/camera_models/camera_fov.cpp new file mode 100644 index 0000000..96543a6 --- /dev/null +++ b/src/x/camera_models/camera_fov.cpp @@ -0,0 +1,35 @@ +// +// Created by viciopoli on 09.10.22. +// + +#include "x/camera_models/camera_fov.h" + +#include + +using namespace x; + +void CameraFov::undistort(Feature &feature) { + const double cam_dist_x = feature.getXDist() * cameraParams_.inv_fx_ - cameraParams_.cx_n_; + const double cam_dist_y = feature.getYDist() * cameraParams_.inv_fy_ - cameraParams_.cy_n_; + + const double dist_r = sqrt(cam_dist_x * cam_dist_x + cam_dist_y * cam_dist_y); + + double distortion_factor = 1.0; + if (dist_r > 0.01) { + distortion_factor = inverseTf(dist_r) / dist_r; + } + const double xn = distortion_factor * cam_dist_x; + const double yn = distortion_factor * cam_dist_y; + + feature.setX(xn * cameraParams_.fx_ + cameraParams_.cx_); + feature.setY(yn * cameraParams_.fy_ + cameraParams_.cy_); +} + + +double CameraFov::inverseTf(const double dist) const { + if (cameraParams_.dist_coeff[0] != 0.0) { + return std::tan(dist * cameraParams_.dist_coeff[0]) * s_term_; + } else { + return dist; + } +} diff --git a/src/x/camera_models/camera_model.cpp b/src/x/camera_models/camera_model.cpp new file mode 100644 index 0000000..d5c31c5 --- /dev/null +++ b/src/x/camera_models/camera_model.cpp @@ -0,0 +1,82 @@ +// +// Created by viciopoli on 09.10.22. +// + +#include "x/camera_models/camera_model.h" + +namespace x { + CameraModel::CameraModel(const Camera::Params &cameraParams) : cameraParams_(cameraParams) {} + + void CameraModel::undistort(FeatureList &features) { + for (auto &f: features) { + undistort(f); + } + } + + Matrix CameraModel::getCameraMatrix() const { return cameraParams_.getCameraMatrixEigen(); } + + cv::Matx33d CameraModel::getCVCameraMatrix() const { return cameraParams_.getCameraMatrix(); } + +#ifdef MULTI_UAV + cv::KeyPoint CameraModel::normalize(const cv::KeyPoint &point) const { + return {static_cast(point.pt.x * cameraParams_.inv_fx_ - cameraParams_.cx_n_), + static_cast(point.pt.y * cameraParams_.inv_fy_ - cameraParams_.cy_n_), point.size}; + } + + cv::KeyPoint CameraModel::denormalize(const cv::KeyPoint &point) const { + return {static_cast((point.pt.x + cameraParams_.cx_n_) / cameraParams_.inv_fx_), + static_cast((point.pt.y + cameraParams_.cy_n_) / cameraParams_.inv_fy_), point.size}; + } +#endif + + + Feature CameraModel::normalize(const Feature &feature) const { + Feature normalized_feature(feature.getTimestamp(), + feature.getX() * cameraParams_.inv_fx_ - cameraParams_.cx_n_, + feature.getY() * cameraParams_.inv_fy_ - cameraParams_.cy_n_, 0.0); + normalized_feature.setXDist(feature.getXDist() * cameraParams_.inv_fx_ - cameraParams_.cx_n_); + normalized_feature.setYDist(feature.getYDist() * cameraParams_.inv_fy_ - cameraParams_.cx_n_); + +#ifdef MULTI_UAV + cv::Mat d = feature.getDescriptor(); + assert(!d.empty()); + normalized_feature.setDescriptor(d); +#endif + +#ifdef GT_DEBUG + Vector3 landmark = feature.getLandmark(); + normalized_feature.setLandmark(landmark); +#endif + + return normalized_feature; + } + + Track CameraModel::normalize(const Track &track, const size_t max_size) const { + // Determine output track size + const size_t track_size = track.size(); + size_t size_out; + if (max_size) { + size_out = std::min(max_size, track_size); + } else { + size_out = track_size; + } + Track normalized_track(size_out, Feature(), track.getId()); + const size_t start_idx(track_size - size_out); + for (size_t j = start_idx; j < track_size; ++j) { + normalized_track[j - start_idx] = normalize(track[j]); + } + return normalized_track; + } + + TrackList CameraModel::normalize(const TrackList &tracks, const size_t max_size) const { + const size_t n = tracks.size(); + TrackList normalized_tracks(n, Track()); + + for (size_t i = 0; i < n; ++i) { + normalized_tracks[i] = normalize(tracks[i], max_size); + } + + return normalized_tracks; + } + +} // x \ No newline at end of file diff --git a/src/x/camera_models/camera_none.cpp b/src/x/camera_models/camera_none.cpp new file mode 100644 index 0000000..031b9cc --- /dev/null +++ b/src/x/camera_models/camera_none.cpp @@ -0,0 +1,12 @@ +// +// Created by viciopoli on 09.10.22. +// + +#include "x/camera_models/camera_none.h" + +using namespace x; + +void CameraNone::undistort(Feature &feature) { + feature.setX(feature.getXDist()); + feature.setY(feature.getYDist()); +} diff --git a/src/x/camera_models/camera_radtan.cpp b/src/x/camera_models/camera_radtan.cpp new file mode 100644 index 0000000..7b27f73 --- /dev/null +++ b/src/x/camera_models/camera_radtan.cpp @@ -0,0 +1,19 @@ +// +// Created by viciopoli on 09.10.22. +// + +#include "x/camera_models/camera_radtan.h" + +using namespace x; + +void CameraRadTan::undistort(Feature &feature) { + const std::vector src{feature.getDistPoint2f()}; + + std::vector res; + const cv::Matx33d m = cameraParams_.getCameraMatrix(); + + cv::undistortPoints(src, res, m, cameraParams_.dist_coeff); + + feature.setX(res[0].x * cameraParams_.fx_ + cameraParams_.cx_); + feature.setY(res[0].y * cameraParams_.fy_ + cameraParams_.cy_); +} diff --git a/src/x/vio/track_manager.cpp b/src/x/vio/track_manager.cpp index 255f650..9462a56 100644 --- a/src/x/vio/track_manager.cpp +++ b/src/x/vio/track_manager.cpp @@ -25,16 +25,16 @@ using namespace x; TrackManager::TrackManager() {} -TrackManager::TrackManager(const Camera &camera, const double min_baseline_x_n, +TrackManager::TrackManager(std::shared_ptr camera, const double min_baseline_x_n, const double min_baseline_y_n) : camera_(camera), min_baseline_x_n_(min_baseline_x_n), min_baseline_y_n_(min_baseline_y_n) {} -void TrackManager::setCamera(Camera camera) { camera_ = camera; } +void TrackManager::setCamera(std::shared_ptr camera) { camera_ = camera; } TrackList TrackManager::normalizeSlamTracks(const int size_out) const { - return camera_.normalize(slam_trks_, size_out); + return camera_->normalize(slam_trks_, size_out); } TrackList TrackManager::getMsckfTracks() const { return msckf_trks_n_; } @@ -57,7 +57,7 @@ TrackList TrackManager::getOppTracks() { // throw std::runtime_error("Opp track contains empty track."); // } // } - return camera_.normalize(opp_trks_, opp_trks_.size()); + return camera_->normalize(opp_trks_, opp_trks_.size()); } #ifdef MULTI_UAV @@ -65,7 +65,7 @@ FeatureList TrackManager::getOppFeatures() const { FeatureList feature_list; for (size_t i = 0; i < opp_trks_.size(); i++) { if (!opp_trks_[i].empty()) - feature_list.emplace_back(camera_.normalize(opp_trks_[i].back())); + feature_list.emplace_back(camera_->normalize(opp_trks_[i].back())); } return feature_list; } @@ -250,7 +250,7 @@ void TrackManager::manageTracks(MatchList &matches, const AttitudeList cam_rots, // list) collab_opp_featrues_print_.push_back(dead_track.back()); msckf_trks_short_n_.emplace_back( - camera_.normalize(dead_track, cam_rots.size())); + camera_->normalize(dead_track, cam_rots.size())); opp_ids_->erase(opp_ids_->begin() + o); count_multi_msckf_++; @@ -265,7 +265,7 @@ void TrackManager::manageTracks(MatchList &matches, const AttitudeList cam_rots, if (a.size() < 2) { continue; } - Track normalized_track = camera_.normalize(a, cam_rots_short.size()); + Track normalized_track = camera_->normalize(a, cam_rots_short.size()); if (checkBaseline(normalized_track, cam_rots_short)) { msckf_trks_short_n_.push_back(normalized_track); } @@ -291,7 +291,7 @@ void TrackManager::manageTracks(MatchList &matches, const AttitudeList cam_rots, // list) collab_opp_featrues_print_.push_back(opp_trks_[t].back()); msckf_trks_n_.emplace_back( - camera_.normalize(opp_trks_[t], cam_rots.size())); + camera_->normalize(opp_trks_[t], cam_rots.size())); opp_trks_.erase(opp_trks_.begin() + t); opp_ids_->erase(opp_ids_->begin() + o); @@ -377,7 +377,7 @@ void TrackManager::manageTracks(MatchList &matches, const AttitudeList cam_rots, // Normalize track (and crop it if it longer than the attitude // list) Track normalized_track = - camera_.normalize(opp_trks_[t], cam_rots.size()); + camera_->normalize(opp_trks_[t], cam_rots.size()); if (checkBaseline(normalized_track, cam_rots)) { msckf_trks_n_.push_back(normalized_track); msckf_featrues_print_.push_back(opp_trks_[t].back()); @@ -409,7 +409,7 @@ void TrackManager::manageTracks(MatchList &matches, const AttitudeList cam_rots, // Normalize coordinates const Track trk = new_slam_trks_[i]; // Track cannot be longer than the attitude list - const Track normalized_trk = camera_.normalize(trk, cam_rots.size()); + const Track normalized_trk = camera_->normalize(trk, cam_rots.size()); // Check baseline and sort if (checkBaseline(normalized_trk, cam_rots)) { @@ -698,14 +698,14 @@ void TrackManager::plotFeatures(TiledImage &img, const int min_track_length) { std::to_string(msckf_trks_short_n_.size()); std::string pot_str = std::string(" - Pot: ") + std::to_string(count_pot); - cv::putText(img, slam_str, cv::Point((int)10, (int)camera_.getHeight() - 10), + cv::putText(img, slam_str, cv::Point((int)10, (int)camera_->getHeight() - 10), cv::FONT_HERSHEY_PLAIN, 1.0, green, 1.5, 8, false); #ifdef MULTI_UAV std::string msckf_multi_str = std::string("Multi OPP-OPP: ") + std::to_string(count_multi_msckf_); cv::putText(img, msckf_multi_str, - cv::Point((int)10, (int)camera_.getHeight() - 25), + cv::Point((int)10, (int)camera_->getHeight() - 25), cv::FONT_HERSHEY_PLAIN, 1.0, red, 1.5, 8, false); #endif @@ -714,18 +714,18 @@ void TrackManager::plotFeatures(TiledImage &img, const int min_track_length) { cv::getTextSize(slam_str, cv::FONT_HERSHEY_PLAIN, 1.0, 1.5, &baseline); int offset = textSize.width; cv::putText(img, opp_str, - cv::Point((int)10 + offset, (int)camera_.getHeight() - 10), + cv::Point((int)10 + offset, (int)camera_->getHeight() - 10), cv::FONT_HERSHEY_PLAIN, 1.0, orange, 1.5, 8, false); textSize = cv::getTextSize(opp_str, cv::FONT_HERSHEY_PLAIN, 1.0, 1.5, &baseline); offset += textSize.width; cv::putText(img, msckf_str, - cv::Point((int)10 + offset, (int)camera_.getHeight() - 10), + cv::Point((int)10 + offset, (int)camera_->getHeight() - 10), cv::FONT_HERSHEY_PLAIN, 1.0, purple, 1.5, 8, false); textSize = cv::getTextSize(msckf_str, cv::FONT_HERSHEY_PLAIN, 1.0, 1.5, &baseline); offset += textSize.width; cv::putText(img, pot_str, - cv::Point((int)10 + offset, (int)camera_.getHeight() - 10), + cv::Point((int)10 + offset, (int)camera_->getHeight() - 10), cv::FONT_HERSHEY_PLAIN, 1.0, blue, 1.5, 8, false); } diff --git a/src/x/vio/vio.cpp b/src/x/vio/vio.cpp index 7b20274..018b8ea 100644 --- a/src/x/vio/vio.cpp +++ b/src/x/vio/vio.cpp @@ -118,9 +118,7 @@ void VIO::setUp(const Params ¶ms) { initialize_start_ = self_init_start_; // Initialize camera geometry - camera_ = - Camera(params_.cam_fx, params_.cam_fy, params_.cam_cx, params_.cam_cy, - params_.cam_s, params_.img_width, params_.img_height); + camera_ = params_.camera; if (params_.min_track_length > params_.n_poses_max) { throw std::invalid_argument( @@ -160,9 +158,9 @@ void VIO::setUp(const Params ¶ms) { // Compute minimum MSCKF baseline in normal plane (square-pixel assumption // along x and y msckf_baseline_x_n_ = - params_.msckf_baseline / (params_.img_width * params_.cam_fx); + params_.msckf_baseline / (camera_->getFx()); msckf_baseline_y_n_ = - params_.msckf_baseline / (params_.img_height * params_.cam_fy); + params_.msckf_baseline / (camera_->getFy()); // Set up tracker and track manager track_manager_ = @@ -205,7 +203,7 @@ void VIO::setUp(const Params ¶ms) { ci_slam_w, params_.iekf_iter); // EKF setup - const size_t state_buffer_sz = params_.state_buffer_size; + const size_t state_buffer_sz = 250; // TODO(jeff) Read from params const State default_state = State(n_poses_state, n_features_state); const double a_m_max = 50.0; const unsigned int delta_seq_imu = 1; @@ -287,11 +285,11 @@ std::optional VIO::processMatchesMeasurement( // Compute 2D image coordinates of the LRF impact point on the ground Feature lrf_img_pt; - lrf_img_pt.setXDist(static_cast((camera_.getWidth() + 1) / 2.0)); - lrf_img_pt.setYDist(static_cast((camera_.getHeight() + 1) / 2.0)); - camera_.undistort(lrf_img_pt); + lrf_img_pt.setXDist(static_cast((camera_->getWidth() + 1) / 2.0)); + lrf_img_pt.setYDist(static_cast((camera_->getHeight() + 1) / 2.0)); + camera_->undistort(lrf_img_pt); last_range_measurement_.img_pt = lrf_img_pt; - last_range_measurement_.img_pt_n = camera_.normalize(lrf_img_pt); + last_range_measurement_.img_pt_n = camera_->normalize(lrf_img_pt); // Pass measurement data to updater VioMeasurement measurement(timestamp_corrected, seq, matches, feature_img, @@ -403,11 +401,11 @@ MatchList VIO::importMatches(const std::vector &match_vector, // Features and match initializations Feature previous_feature(match_vector[feature_arr_blk_sz * i + 1], seq - 1, 0.0, 0.0, x_dist_prev, y_dist_prev, -1.0); - camera_.undistort(previous_feature); + camera_->undistort(previous_feature); Feature current_feature(match_vector[feature_arr_blk_sz * i + 4], seq, 0.0, 0.0, x_dist_curr, y_dist_curr, -1.0); - camera_.undistort(current_feature); + camera_->undistort(current_feature); #ifdef GT_DEBUG // 3D landmark given by the ground truth @@ -608,14 +606,22 @@ Params VIO::loadParamsFromYaml(fsm::path& filePath) { params.sigma_dbw << sigma_dbw[0], sigma_dbw[1], sigma_dbw[2]; params.sigma_dba << sigma_dba[0], sigma_dba[1], sigma_dba[2]; - file["cam1_fx"] >> params.cam_fx; - file["cam1_fy"] >> params.cam_fy; - file["cam1_cx"] >> params.cam_cx; - file["cam1_cy"] >> params.cam_cy; - file["cam1_s"] >> params.cam_s; - file["cam1_img_height"] >> params.img_height; - file["cam1_img_width"] >> params.img_width; + double cam_fx, cam_fy, cam_cx, cam_cy, img_height, img_width; + std::vector dist_coeffs; + std::string distortion_model; + file["cam1_fx"] >> cam_fx; + file["cam1_fy"] >> cam_fy; + file["cam1_cx"] >> cam_cx; + file["cam1_cy"] >> cam_cy; + file["cam1_dist"] >> dist_coeffs; + file["cam1_img_height"] >> img_height; + file["cam1_img_width"] >> img_width; + file["distortion_model"] >> distortion_model; + + Camera::Params camera_params(cam_fx, cam_fy, cam_cx, cam_cy, dist_coeffs, img_width, img_height, distortion_model); + params.camera = Camera::constructCamera(camera_params); + file["cam1_p_ic"] >> p_ic; file["cam1_q_ic"] >> q_ic; diff --git a/src/x/vision/camera.cpp b/src/x/vision/camera.cpp deleted file mode 100644 index f6540d2..0000000 --- a/src/x/vision/camera.cpp +++ /dev/null @@ -1,169 +0,0 @@ -/* - * Copyright 2020 California Institute of Technology (“Caltech”) - * - * Licensed under 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. - */ - -#include "x/vision/camera.h" - -#include -#include -#include - -using namespace x; - -Camera::Camera() = default; - -Camera::Camera(double fx, double fy, double cx, double cy, double s, - unsigned int img_width, unsigned int img_height) - : s_(s), img_width_(img_width), img_height_(img_height) { - fx_ = img_width * fx; - fy_ = img_height * fy; - cx_ = img_width * cx; - cy_ = img_height * cy; - - inv_fx_ = 1.0 / fx_; - inv_fy_ = 1.0 / fy_; - cx_n_ = cx_ * inv_fx_; - cy_n_ = cy_ * inv_fy_; - s_term_ = 1.0 / (2.0 * std::tan(s / 2.0)); - - K_ = Matrix::Zero(3, 3); - K_(0, 0) = fx_; - K_(1, 1) = fy_; - K_(0, 2) = cx_; - K_(1, 2) = cy_; - K_(2, 2) = 1.0; - cv::eigen2cv(K_, cvK_); -} - -unsigned int Camera::getWidth() const { return img_width_; } - -unsigned int Camera::getHeight() const { return img_height_; } - -double Camera::getInvFx() const { return inv_fx_; } - -double Camera::getInvFy() const { return inv_fy_; } - -double Camera::getCxN() const { return cx_n_; } - -double Camera::getCyN() const { return cy_n_; } - -void Camera::undistort(FeatureList &features) const { - // Undistort each point in the input vector - for (auto &feature : features) { - undistort(feature); - } -} - -void Camera::undistort(Feature &feature) const { - // cv::Point2d p = - // LUT_calib_[feature.getYDist() * img_width_ + feature.getXDist()]; - // feature.setX(p.x); - // feature.setY(p.y); - const double cam_dist_x = feature.getXDist() * inv_fx_ - cx_n_; - const double cam_dist_y = feature.getYDist() * inv_fy_ - cy_n_; - - const double dist_r = sqrt(cam_dist_x * cam_dist_x + cam_dist_y * cam_dist_y); - - double distortion_factor = 1.0; - if (dist_r > 0.01) distortion_factor = inverseTf(dist_r) / dist_r; - - const double xn = distortion_factor * cam_dist_x; - const double yn = distortion_factor * cam_dist_y; - - feature.setX(xn * fx_ + cx_); - feature.setY(yn * fy_ + cy_); -} - -Matrix Camera::getCameraMatrix() const { return K_; } -cv::Mat Camera::getCVCameraMatrix() const { return cvK_; } -#ifdef MULTI_UAV - -cv::KeyPoint Camera::normalize(const cv::KeyPoint &point) const { - return {static_cast(point.pt.x * inv_fx_ - cx_n_), - static_cast(point.pt.y * inv_fy_ - cy_n_), point.size}; -} -cv::KeyPoint Camera::denormalize(const cv::KeyPoint &point) const { - return {static_cast((point.pt.x + cx_n_) / inv_fx_), - static_cast((point.pt.y + cy_n_) / inv_fy_), point.size}; -} -#endif - -Feature Camera::normalize(const Feature &feature) const { - Feature normalized_feature( - feature.getTimestamp(), feature.getX() * inv_fx_ - cx_n_, - feature.getY() * inv_fy_ - cy_n_, feature.getIntensity()); - -#ifdef MULTI_UAV - cv::Mat d = feature.getDescriptor(); - assert(!d.empty()); - normalized_feature.setDescriptor(d); -#endif - -#ifdef GT_DEBUG - Vector3 landmark = feature.getLandmark(); - normalized_feature.setLandmark(landmark); -#endif - - normalized_feature.setXDist(feature.getXDist() * inv_fx_ - cx_n_); - normalized_feature.setYDist(feature.getYDist() * inv_fy_ - cy_n_); - - return normalized_feature; -} - -/** \brief Normalized the image coordinates for all features in the input track - * \param track Track to normalized - * \param max_size Max size of output track, cropping from the end (default 0: - * no cropping) \return Normalized track - */ -Track Camera::normalize(const Track &track, const size_t max_size) const { - // Determine output track size - const size_t track_size = track.size(); - size_t size_out; - if (max_size) - size_out = std::min(max_size, track_size); - else - size_out = track_size; - - Track normalized_track(size_out, Feature(), track.getId()); - const size_t start_idx(track_size - size_out); - for (size_t j = start_idx; j < track_size; ++j) { - normalized_track[j - start_idx] = normalize(track[j]); - } - return normalized_track; -} - -/** \brief Normalized the image coordinates for all features in the input track - * list \param tracks List of tracks \param max_size Max size of output tracks, - * cropping from the end (default 0: no cropping) \return Normalized list of - * tracks - */ -TrackList Camera::normalize(const TrackList &tracks, - const size_t max_size) const { - const size_t n = tracks.size(); - TrackList normalized_tracks; - - for (size_t i = 0; i < n; ++i) - normalized_tracks.push_back(normalize(tracks[i], max_size)); - - return normalized_tracks; -} - -double Camera::inverseTf(const double &dist) const { - if (s_ == 0.0) { - return dist; - } else { - return std::tan(dist * s_) * s_term_; - } -} diff --git a/src/x/vision/tracker.cpp b/src/x/vision/tracker.cpp index ca42404..8b227c6 100644 --- a/src/x/vision/tracker.cpp +++ b/src/x/vision/tracker.cpp @@ -29,7 +29,7 @@ Tracker::~Tracker() {} Tracker::Tracker() = default; -Tracker::Tracker(const Camera &cam, const int fast_detection_delta, +Tracker::Tracker(const std::shared_ptr cam, const int fast_detection_delta, const bool non_max_supp, const unsigned int block_half_length, unsigned int margin, unsigned int n_feat_min, int outlier_method, double outlier_param1, @@ -87,7 +87,7 @@ Tracker::Tracker(const Camera &cam, const int fast_detection_delta, } void Tracker::setParams( - const Camera &cam, const int fast_detection_delta, const bool non_max_supp, + const std::shared_ptr cam, const int fast_detection_delta, const bool non_max_supp, const unsigned int block_half_length, const unsigned int margin, const unsigned int n_feat_min, const int outlier_method, const double outlier_param1, const double outlier_param2, @@ -166,7 +166,7 @@ void Tracker::track(TiledImage ¤t_img, const double ×tamp, featureDetection(current_img, current_features, timestamp, frame_number, old_features); // Undistortion of current features - camera_.undistort(current_features); + camera_->undistort(current_features); #ifdef TIMING clock3 = clock(); @@ -231,10 +231,10 @@ void Tracker::track(TiledImage ¤t_img, const double ×tamp, clock5 = clock(); #endif // Undistortion of current features - camera_.undistort(current_features); + camera_->undistort(current_features); // Undistort features in the previous image // TODO(jeff) only do it for new features newly detected - camera_.undistort(previous_features_); + camera_->undistort(previous_features_); //========================================================================== // Outlier removal