From a251cab4cd3d0cbf5b03e866f569b03a96987f88 Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Fri, 27 Dec 2024 11:12:05 +0100 Subject: [PATCH] add Equirectangular camera --- src/aliceVision/aliceVision.i | 1 + src/aliceVision/camera/CMakeLists.txt | 2 + src/aliceVision/camera/Camera.i | 1 + src/aliceVision/camera/Equirectangular.cpp | 194 +++++++++++++++++++++ src/aliceVision/camera/Equirectangular.hpp | 117 +++++++++++++ src/aliceVision/camera/Equirectangular.i | 17 ++ src/aliceVision/camera/camera.hpp | 6 + src/aliceVision/camera/cameraCommon.hpp | 12 +- 8 files changed, 349 insertions(+), 1 deletion(-) create mode 100644 src/aliceVision/camera/Equirectangular.cpp create mode 100644 src/aliceVision/camera/Equirectangular.hpp create mode 100644 src/aliceVision/camera/Equirectangular.i diff --git a/src/aliceVision/aliceVision.i b/src/aliceVision/aliceVision.i index 31f6814067..8a978d3986 100644 --- a/src/aliceVision/aliceVision.i +++ b/src/aliceVision/aliceVision.i @@ -25,6 +25,7 @@ #include #include #include +#include using namespace aliceVision; diff --git a/src/aliceVision/camera/CMakeLists.txt b/src/aliceVision/camera/CMakeLists.txt index b0490fa027..bf8c1f175c 100644 --- a/src/aliceVision/camera/CMakeLists.txt +++ b/src/aliceVision/camera/CMakeLists.txt @@ -14,6 +14,7 @@ set(camera_files_headers Undistortion3DEClassicLD.hpp UndistortionRadial.hpp Equidistant.hpp + Equirectangular.hpp IntrinsicBase.hpp IntrinsicInitMode.hpp IntrinsicScaleOffset.hpp @@ -28,6 +29,7 @@ set(camera_files_sources DistortionRadial.cpp UndistortionRadial.cpp Equidistant.cpp + Equirectangular.cpp IntrinsicBase.cpp IntrinsicScaleOffset.cpp IntrinsicScaleOffsetDisto.cpp diff --git a/src/aliceVision/camera/Camera.i b/src/aliceVision/camera/Camera.i index 2dc335ca47..862f286763 100644 --- a/src/aliceVision/camera/Camera.i +++ b/src/aliceVision/camera/Camera.i @@ -29,4 +29,5 @@ using namespace aliceVision::camera; %include %include +%include %include diff --git a/src/aliceVision/camera/Equirectangular.cpp b/src/aliceVision/camera/Equirectangular.cpp new file mode 100644 index 0000000000..97244a74a9 --- /dev/null +++ b/src/aliceVision/camera/Equirectangular.cpp @@ -0,0 +1,194 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#include "Equirectangular.hpp" + +#include +#include + +namespace aliceVision { +namespace camera { + +std::shared_ptr Equirectangular::cast(std::shared_ptr sptr) +{ + return std::dynamic_pointer_cast(sptr); +} + +Vec2 Equirectangular::transformProject(const Eigen::Matrix4d& pose, const Vec4& pt, bool applyDistortion) const +{ + Vec4 X = pose * pt; + Vec3 spherical = X.head(3).normalized(); + + Vec2 angles; + angles.x() = atan2(spherical(0), spherical(2)); + angles.y() = asin(spherical(1)); + + Vec2 imapt = cam2ima(angles); + + return imapt; +} + +Vec2 Equirectangular::project(const Vec4& pt, bool applyDistortion) const +{ + Vec3 spherical = pt.head(3).normalized(); + + Vec2 angles; + angles.x() = atan2(spherical(0), spherical(2)); + angles.y() = asin(spherical(1)); + + Vec2 imapt = cam2ima(angles); + + return imapt; +} + +Eigen::Matrix Equirectangular::getDerivativeTransformProjectWrtPoint3(const Eigen::Matrix4d& T, const Vec4& pt) const +{ + Vec3 spherical = pt.head(3).normalized(); + + double sx = spherical(0); + double sy = spherical(1); + double sz = spherical(2); + double len = sx*sx + sy*sy; + + double norm = pt.norm(); + double normsq = norm * norm; + double invnormsq = 1.0 / normsq; + + double d_norm_d_x = pt.x() / norm; + double d_norm_d_y = pt.y() / norm; + double d_norm_d_z = pt.z() / norm; + + // x / norm; y / norm; z / norm + Eigen::Matrix d_spherical_d_pt3; + d_spherical_d_pt3(0, 0) = (norm * 1.0 - pt.x() * d_norm_d_x) * invnormsq; + d_spherical_d_pt3(0, 1) = (norm * 0.0 - pt.x() * d_norm_d_y) * invnormsq; + d_spherical_d_pt3(0, 2) = (norm * 0.0 - pt.x() * d_norm_d_z) * invnormsq; + d_spherical_d_pt3(1, 0) = (norm * 0.0 - pt.y() * d_norm_d_x) * invnormsq; + d_spherical_d_pt3(1, 1) = (norm * 1.0 - pt.y() * d_norm_d_y) * invnormsq; + d_spherical_d_pt3(1, 2) = (norm * 0.0 - pt.y() * d_norm_d_z) * invnormsq; + d_spherical_d_pt3(2, 0) = (norm * 0.0 - pt.z() * d_norm_d_x) * invnormsq; + d_spherical_d_pt3(2, 1) = (norm * 0.0 - pt.z() * d_norm_d_y) * invnormsq; + d_spherical_d_pt3(2, 2) = (norm * 1.0 - pt.z() * d_norm_d_z) * invnormsq; + + + //atan2(spherical(0), spherical(2)) ; asin(spherical(1)); + Eigen::Matrix d_coords_d_spherical = Eigen::Matrix::Zero(); + d_coords_d_spherical(0, 0) = sz / len; + d_coords_d_spherical(0, 2) = - sx / len; + d_coords_d_spherical(1, 1) = 1.0 / sqrt(1.0 - (sy*sy)); + + + return getDerivativeCam2ImaWrtPoint() * d_coords_d_spherical * d_spherical_d_pt3; +} + +Eigen::Matrix Equirectangular::getDerivativeTransformProjectWrtDisto(const Eigen::Matrix4d& pose, const Vec4& pt) const +{ + return Eigen::Matrix::Zero(); +} + +Eigen::Matrix Equirectangular::getDerivativeTransformProjectWrtScale(const Eigen::Matrix4d& pose, const Vec4& pt) const +{ + Vec3 spherical = pt.head(3).normalized(); + + Vec2 angles; + angles.x() = atan2(spherical(0), spherical(2)); + angles.y() = asin(spherical(1)); + + return getDerivativeCam2ImaWrtScale(angles); +} + +Eigen::Matrix Equirectangular::getDerivativeTransformProjectWrtPrincipalPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const +{ + return getDerivativeCam2ImaWrtPrincipalPoint(); +} + +Eigen::Matrix Equirectangular::getDerivativeTransformProjectWrtParams(const Eigen::Matrix4d& pose, const Vec4& pt3D) const +{ + Eigen::Matrix ret(2, getParams().size()); + + ret.block<2, 2>(0, 0) = getDerivativeTransformProjectWrtScale(pose, pt3D); + ret.block<2, 2>(0, 2) = getDerivativeTransformProjectWrtPrincipalPoint(pose, pt3D); + + return ret; +} + +Vec3 Equirectangular::toUnitSphere(const Vec2& pt) const +{ + const double latitude = pt(1); + const double longitude = pt(0); + + Vec3 spherical; + spherical.x() = cos(latitude) * sin(longitude); + spherical.y() = sin(latitude); + spherical.z() = cos(latitude) * cos(longitude); + + return spherical; +} + +Eigen::Matrix Equirectangular::getDerivativetoUnitSphereWrtPoint(const Vec2& pt) const +{ + const double latitude = pt(1); + const double longitude = pt(0); + + Vec3 spherical; + spherical.x() = cos(latitude) * sin(longitude); + spherical.y() = sin(latitude); + spherical.z() = cos(latitude) * cos(longitude); + + Eigen::Matrix d_spherical_d_pt; + d_spherical_d_pt(0, 0) = cos(latitude) * cos(longitude); + d_spherical_d_pt(0, 1) = -sin(latitude) * sin(longitude); + d_spherical_d_pt(1, 0) = 0; + d_spherical_d_pt(1, 1) = cos(latitude); + d_spherical_d_pt(0, 0) = cos(latitude) * -sin(longitude); + d_spherical_d_pt(0, 1) = -sin(latitude) * cos(longitude); + + return d_spherical_d_pt; +} + +Eigen::Matrix Equirectangular::getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const +{ + const Vec2 ptMeters = ima2cam(pt2D); + const Vec3 ptSphere = toUnitSphere(ptMeters); + + Eigen::Matrix ret(3, 4); + + Eigen::Matrix J = getDerivativetoUnitSphereWrtPoint(ptMeters); + + ret.block<3, 2>(0, 0) = J * getDerivativeIma2CamWrtScale(pt2D); + ret.block<3, 2>(0, 2) = J * getDerivativeIma2CamWrtPrincipalPoint(); + + return ret; +} + +double Equirectangular::imagePlaneToCameraPlaneError(double value) const { return 0.0; } + +bool Equirectangular::isVisibleRay(const Vec3& ray) const +{ + return true; +} + +EINTRINSIC Equirectangular::getType() const +{ + return EINTRINSIC::EQUIRECTANGULAR_CAMERA; +} + +double Equirectangular::getHorizontalFov() const +{ + return 2.0 * M_PI; +} + +double Equirectangular::getVerticalFov() const { return M_PI; } + +double Equirectangular::pixelProbability() const +{ + return 1.0 / double(w()); +} + + + +} // namespace camera +} // namespace aliceVision diff --git a/src/aliceVision/camera/Equirectangular.hpp b/src/aliceVision/camera/Equirectangular.hpp new file mode 100644 index 0000000000..e57f458652 --- /dev/null +++ b/src/aliceVision/camera/Equirectangular.hpp @@ -0,0 +1,117 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#pragma once + +#include +#include +#include +#include +#include + +#include "DistortionFisheye1.hpp" + +#include +#include + +namespace aliceVision { +namespace camera { + +/** + * @brief Equirectangular is a camera model used for panoramas. + * See https://en.wikipedia.org/wiki/Equirectangular_projection + * + */ +class Equirectangular : public IntrinsicScaleOffsetDisto +{ + public: + Equirectangular() + : Equirectangular(1, 1, 1.0, 1.0, 0.0, 0.0) + {} + + Equirectangular(unsigned int w, + unsigned int h, + double focalLengthPixX, + double focalLengthPixY, + double offsetX, + double offsetY) + : IntrinsicScaleOffsetDisto(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY) + {} + + + ~Equirectangular() override = default; + + Equirectangular* clone() const override { return new Equirectangular(*this); } + + static std::shared_ptr cast(std::shared_ptr sptr); + + void assign(const IntrinsicBase& other) override { *this = dynamic_cast(other); } + + bool isValid() const override { return _scale(0) > 0 && IntrinsicBase::isValid(); } + + EINTRINSIC getType() const override; + + Vec2 transformProject(const Eigen::Matrix4d& pose, const Vec4& pt, bool applyDistortion = true) const override; + + Vec2 transformProject(const geometry::Pose3& pose, const Vec4& pt3D, bool applyDistortion = true) const + { + return transformProject(pose.getHomogeneous(), pt3D, applyDistortion); + } + + Vec2 project(const Vec4& pt, bool applyDistortion = true) const override; + + Eigen::Matrix getDerivativeTransformProjectWrtPoint3(const Eigen::Matrix4d& pose, const Vec4& pt) const override; + + Eigen::Matrix getDerivativeTransformProjectWrtDisto(const Eigen::Matrix4d& pose, const Vec4& pt) const; + + Eigen::Matrix getDerivativeTransformProjectWrtScale(const Eigen::Matrix4d& pose, const Vec4& pt) const; + + Eigen::Matrix getDerivativeTransformProjectWrtPrincipalPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const; + + Eigen::Matrix getDerivativeTransformProjectWrtParams(const Eigen::Matrix4d& pose, const Vec4& pt3D) const override; + + Vec3 toUnitSphere(const Vec2& pt) const override; + + Eigen::Matrix getDerivativetoUnitSphereWrtPoint(const Vec2& pt) const; + + /** + * @brief Get the derivative of the unit sphere backprojection + * @param[in] pt2D The 2D point + * @return The backproject jacobian with respect to the pose + */ + Eigen::Matrix getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const override; + + double imagePlaneToCameraPlaneError(double value) const override; + + /** + * @brief Return true if this ray should be visible in the image + * @param[in] ray the ray that may or may not be visible in the image + * @return True if this ray is visible theoretically, false otherwise + */ + bool isVisibleRay(const Vec3& ray) const override; + + /** + * @brief Get the horizontal FOV in radians + * @return Horizontal FOV in radians + */ + double getHorizontalFov() const override; + + /** + * @brief Get the vertical FOV in radians + * @return Vertical FOV in radians + */ + double getVerticalFov() const override; + + + /** + * @brief how a one pixel change relates to an angular change + * @return a value in radians + */ + double pixelProbability() const override; +}; + +} // namespace camera +} // namespace aliceVision diff --git a/src/aliceVision/camera/Equirectangular.i b/src/aliceVision/camera/Equirectangular.i new file mode 100644 index 0000000000..24f47478e1 --- /dev/null +++ b/src/aliceVision/camera/Equirectangular.i @@ -0,0 +1,17 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +%include +%shared_ptr(aliceVision::camera::Equirectangular) + +%include +%include + +%{ +#include +using namespace aliceVision; +using namespace aliceVision::camera; +%} diff --git a/src/aliceVision/camera/camera.hpp b/src/aliceVision/camera/camera.hpp index c1c60ab14f..476ddad309 100644 --- a/src/aliceVision/camera/camera.hpp +++ b/src/aliceVision/camera/camera.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -156,6 +157,11 @@ inline std::shared_ptr createIntrinsic(EINTRINSIC intrinsicType, return std::make_shared(w, h, focalLengthPixX, offsetX, offsetY, distortion); } + if (isEquirectangular(intrinsicType)) + { + return std::make_shared(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY); + } + return nullptr; } diff --git a/src/aliceVision/camera/cameraCommon.hpp b/src/aliceVision/camera/cameraCommon.hpp index 574faa1ad6..dd364f0509 100644 --- a/src/aliceVision/camera/cameraCommon.hpp +++ b/src/aliceVision/camera/cameraCommon.hpp @@ -44,6 +44,7 @@ enum EINTRINSIC UNKNOWN = (1u << 0), PINHOLE_CAMERA = (1u << 1), // plain pinhole model EQUIDISTANT_CAMERA = (1u << 2), // plain equidistant model + EQUIRECTANGULAR_CAMERA = (1u << 3), // plain equirectangular model }; BOOST_BITMASK(EINTRINSIC); @@ -56,6 +57,8 @@ inline std::string EINTRINSIC_enumToString(EINTRINSIC intrinsic) return "pinhole"; case EINTRINSIC::EQUIDISTANT_CAMERA: return "equidistant"; + case EINTRINSIC::EQUIRECTANGULAR_CAMERA: + return "equirectangular"; case EINTRINSIC::UNKNOWN: break; } @@ -77,6 +80,11 @@ inline EINTRINSIC EINTRINSIC_stringToEnum(const std::string& intrinsic) return EINTRINSIC::EQUIDISTANT_CAMERA; } + if (type == "equirectangular") + { + return EINTRINSIC::EQUIRECTANGULAR_CAMERA; + } + throw std::out_of_range(intrinsic); } @@ -93,8 +101,10 @@ inline bool isPinhole(EINTRINSIC eintrinsic) { return EINTRINSIC::PINHOLE_CAMERA inline bool isEquidistant(EINTRINSIC eintrinsic) { return EINTRINSIC::EQUIDISTANT_CAMERA & eintrinsic; } +inline bool isEquirectangular(EINTRINSIC eintrinsic) { return EINTRINSIC::EQUIRECTANGULAR_CAMERA & eintrinsic; } + // Return if the camera type is a valid enum -inline bool isValid(EINTRINSIC eintrinsic) { return isPinhole(eintrinsic) || isEquidistant(eintrinsic); } +inline bool isValid(EINTRINSIC eintrinsic) { return isPinhole(eintrinsic) || isEquidistant(eintrinsic) || isEquirectangular(eintrinsic); } inline EDISTORTION EDISTORTION_stringToEnum(const std::string& distortion) {