add Equirectangular camera

src/aliceVision/aliceVision.i

@@ -25,6 +25,7 @@
 #include <aliceVision/camera/IntrinsicBase.hpp>
 #include <aliceVision/camera/Pinhole.hpp>
 #include <aliceVision/camera/Equidistant.hpp>
+#include <aliceVision/camera/Equirectangular.hpp>
 
 using namespace aliceVision;
 

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

src/aliceVision/camera/Camera.i

@@ -29,4 +29,5 @@ using namespace aliceVision::camera;
 
 %include <aliceVision/camera/IntrinsicInitMode.i>
 %include <aliceVision/camera/Equidistant.i>
+%include <aliceVision/camera/Equirectangular.i>
 %include <aliceVision/camera/Pinhole.i>

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 <algorithm>
+#include <cmath>
+
+namespace aliceVision {
+namespace camera {
+
+std::shared_ptr<Equirectangular> Equirectangular::cast(std::shared_ptr<IntrinsicBase> sptr)
+{
+    return std::dynamic_pointer_cast<Equirectangular>(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<double, 2, 3> 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<double, 3, 3> 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<double, 2, 3> d_coords_d_spherical = Eigen::Matrix<double, 2, 3>::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<double, 2, 3> Equirectangular::getDerivativeTransformProjectWrtDisto(const Eigen::Matrix4d& pose, const Vec4& pt) const
+{
+    return Eigen::Matrix<double, 2, 3>::Zero();
+}
+
+Eigen::Matrix<double, 2, 2> 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<double, 2, 2> Equirectangular::getDerivativeTransformProjectWrtPrincipalPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const
+{
+    return getDerivativeCam2ImaWrtPrincipalPoint();
+}
+
+Eigen::Matrix<double, 2, Eigen::Dynamic> Equirectangular::getDerivativeTransformProjectWrtParams(const Eigen::Matrix4d& pose, const Vec4& pt3D) const
+{
+    Eigen::Matrix<double, 2, Eigen::Dynamic> 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<double, 3, 2> 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<double, 3, 2> 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<double, 3, Eigen::Dynamic> Equirectangular::getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const 
+{
+    const Vec2 ptMeters = ima2cam(pt2D);
+    const Vec3 ptSphere = toUnitSphere(ptMeters);
+
+    Eigen::Matrix<double, 3, Eigen::Dynamic> ret(3, 4);
+
+    Eigen::Matrix<double, 3, 2> 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

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 <aliceVision/numeric/numeric.hpp>
+#include <aliceVision/numeric/projection.hpp>
+#include <aliceVision/camera/cameraCommon.hpp>
+#include <aliceVision/camera/IntrinsicScaleOffsetDisto.hpp>
+#include <aliceVision/geometry/Pose3.hpp>
+
+#include "DistortionFisheye1.hpp"
+
+#include <memory>
+#include <algorithm>
+
+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<Equirectangular> cast(std::shared_ptr<IntrinsicBase> sptr);
+
+    void assign(const IntrinsicBase& other) override { *this = dynamic_cast<const Equirectangular&>(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<double, 2, 3> getDerivativeTransformProjectWrtPoint3(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
+
+    Eigen::Matrix<double, 2, 3> getDerivativeTransformProjectWrtDisto(const Eigen::Matrix4d& pose, const Vec4& pt) const;
+
+    Eigen::Matrix<double, 2, 2> getDerivativeTransformProjectWrtScale(const Eigen::Matrix4d& pose, const Vec4& pt) const;
+
+    Eigen::Matrix<double, 2, 2> getDerivativeTransformProjectWrtPrincipalPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const;
+
+    Eigen::Matrix<double, 2, Eigen::Dynamic> getDerivativeTransformProjectWrtParams(const Eigen::Matrix4d& pose, const Vec4& pt3D) const override;
+
+    Vec3 toUnitSphere(const Vec2& pt) const override;
+
+    Eigen::Matrix<double, 3, 2> 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<double, 3, Eigen::Dynamic> 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

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 <std_shared_ptr.i>
+%shared_ptr(aliceVision::camera::Equirectangular)
+
+%include <aliceVision/camera/IntrinsicScaleOffsetDisto.i>
+%include <aliceVision/camera/Equirectangular.hpp>
+
+%{
+#include <aliceVision/camera/Equirectangular.hpp>
+using namespace aliceVision;
+using namespace aliceVision::camera;
+%}

src/aliceVision/camera/camera.hpp

@@ -21,6 +21,7 @@
 #include <aliceVision/camera/IntrinsicBase.hpp>
 #include <aliceVision/camera/Pinhole.hpp>
 #include <aliceVision/camera/Equidistant.hpp>
+#include <aliceVision/camera/Equirectangular.hpp>
 #include <aliceVision/camera/cameraUndistortImage.hpp>
 
 #include <memory>
@@ -156,6 +157,11 @@ inline std::shared_ptr<IntrinsicBase> createIntrinsic(EINTRINSIC intrinsicType,
         return std::make_shared<Equidistant>(w, h, focalLengthPixX, offsetX, offsetY, distortion);
     }
 
+    if (isEquirectangular(intrinsicType))
+    {
+        return std::make_shared<Equirectangular>(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY);
+    }
+
     return nullptr;
 }