Merge pull request #1801 from alicevision/dev/equirectangular

Add Equirectangular camera support for SFM

Author: cbentejac

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/calibration/distortionEstimationGeometry.cpp

@@ -7,6 +7,7 @@
 #include "distortionEstimationGeometry.hpp"
 
 #include <aliceVision/system/Logger.hpp>
+#include <aliceVision/numeric/algebra.hpp>
 #include <aliceVision/sfm/bundle/manifolds/so3.hpp>
 #include <ceres/ceres.h>
 #include <cmath>

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/Equidistant.cpp

@@ -67,203 +67,6 @@ Vec2 Equidistant::project(const Vec4& pt, bool applyDistortion) const
     return pt_ima;
 }
 
-Eigen::Matrix<double, 2, 9> Equidistant::getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const
-{
-    Eigen::Matrix4d T = pose;
-    const Vec4 X = T * pt;  // apply pose
-
-    const Eigen::Matrix<double, 3, 9> d_X_d_R = getJacobian_AB_wrt_A<3, 3, 1>(pose.block<3, 3>(0, 0), pt.head(3));
-
-    /* Compute angle with optical center */
-    double len2d = sqrt(X(0) * X(0) + X(1) * X(1));
-    Eigen::Matrix<double, 2, 2> d_len2d_d_X;
-    d_len2d_d_X(0) = X(0) / len2d;
-    d_len2d_d_X(1) = X(1) / len2d;
-
-    const double angle_Z = std::atan2(len2d, X(2));
-    const double d_angle_Z_d_len2d = X(2) / (len2d * len2d + X(2) * X(2));
-
-    /* Ignore depth component and compute radial angle */
-    const double angle_radial = std::atan2(X(1), X(0));
-
-    Eigen::Matrix<double, 2, 3> d_angles_d_X;
-    d_angles_d_X(0, 0) = -X(1) / (X(0) * X(0) + X(1) * X(1));
-    d_angles_d_X(0, 1) = X(0) / (X(0) * X(0) + X(1) * X(1));
-    d_angles_d_X(0, 2) = 0.0;
-
-    d_angles_d_X(1, 0) = d_angle_Z_d_len2d * d_len2d_d_X(0);
-    d_angles_d_X(1, 1) = d_angle_Z_d_len2d * d_len2d_d_X(1);
-    d_angles_d_X(1, 2) = -len2d / (len2d * len2d + X(2) * X(2));
-
-    const double rsensor = std::min(sensorWidth(), sensorHeight());
-    const double rscale = sensorWidth() / std::max(w(), h());
-    const double fmm = _scale(0) * rscale;
-    const double fov = rsensor / fmm;
-
-    const double radius = angle_Z / (0.5 * fov);
-
-    const double d_radius_d_angle_Z = 1.0 / (0.5 * fov);
-
-    /* radius = focal * angle_Z */
-    const Vec2 P{cos(angle_radial) * radius, sin(angle_radial) * radius};
-
-    Eigen::Matrix<double, 2, 2> d_P_d_angles;
-    d_P_d_angles(0, 0) = -sin(angle_radial) * radius;
-    d_P_d_angles(0, 1) = cos(angle_radial) * d_radius_d_angle_Z;
-    d_P_d_angles(1, 0) = cos(angle_radial) * radius;
-    d_P_d_angles(1, 1) = sin(angle_radial) * d_radius_d_angle_Z;
-
-    return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_angles * d_angles_d_X * d_X_d_R;
-}
-
-Eigen::Matrix<double, 2, 16> Equidistant::getDerivativeTransformProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt) const
-{
-    Eigen::Matrix4d T = pose;
-    const Vec4 X = T * pt;  // apply pose
-
-    const Eigen::Matrix<double, 4, 16> d_X_d_T = getJacobian_AB_wrt_A<4, 4, 1>(T, pt);
-
-    /* Compute angle with optical center */
-    double len2d = sqrt(X(0) * X(0) + X(1) * X(1));
-    Eigen::Matrix<double, 2, 2> d_len2d_d_X;
-    d_len2d_d_X(0) = X(0) / len2d;
-    d_len2d_d_X(1) = X(1) / len2d;
-
-    const double angle_Z = std::atan2(len2d, X(2));
-    const double d_angle_Z_d_len2d = X(2) / (len2d * len2d + X(2) * X(2));
-
-    /* Ignore depth component and compute radial angle */
-    const double angle_radial = std::atan2(X(1), X(0));
-
-    Eigen::Matrix<double, 2, 3> d_angles_d_X;
-    d_angles_d_X(0, 0) = -X(1) / (X(0) * X(0) + X(1) * X(1));
-    d_angles_d_X(0, 1) = X(0) / (X(0) * X(0) + X(1) * X(1));
-    d_angles_d_X(0, 2) = 0.0;
-
-    d_angles_d_X(1, 0) = d_angle_Z_d_len2d * d_len2d_d_X(0);
-    d_angles_d_X(1, 1) = d_angle_Z_d_len2d * d_len2d_d_X(1);
-    d_angles_d_X(1, 2) = -len2d / (len2d * len2d + X(2) * X(2));
-
-    const double rsensor = std::min(sensorWidth(), sensorHeight());
-    const double rscale = sensorWidth() / std::max(w(), h());
-    const double fmm = _scale(0) * rscale;
-    const double fov = rsensor / fmm;
-
-    const double radius = angle_Z / (0.5 * fov);
-
-    const double d_radius_d_angle_Z = 1.0 / (0.5 * fov);
-
-    /* radius = focal * angle_Z */
-    const Vec2 P{cos(angle_radial) * radius, sin(angle_radial) * radius};
-
-    Eigen::Matrix<double, 2, 2> d_P_d_angles;
-    d_P_d_angles(0, 0) = -sin(angle_radial) * radius;
-    d_P_d_angles(0, 1) = cos(angle_radial) * d_radius_d_angle_Z;
-    d_P_d_angles(1, 0) = cos(angle_radial) * radius;
-    d_P_d_angles(1, 1) = sin(angle_radial) * d_radius_d_angle_Z;
-
-    return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_angles * d_angles_d_X * d_X_d_T.block<3, 16>(0, 0);
-}
-
-Eigen::Matrix<double, 2, 16> Equidistant::getDerivativeTransformProjectWrtPoseLeft(const Eigen::Matrix4d& pose, const Vec4& pt) const
-{
-    Eigen::Matrix4d T = pose;
-    const Vec4 X = T * pt;  // apply pose
-
-    const Eigen::Matrix<double, 4, 16> d_X_d_T = getJacobian_AB_wrt_A<4, 4, 1>(Eigen::Matrix4d::Identity(), X);
-
-    /* Compute angle with optical center */
-    double len2d = sqrt(X(0) * X(0) + X(1) * X(1));
-    Eigen::Matrix<double, 2, 2> d_len2d_d_X;
-    d_len2d_d_X(0) = X(0) / len2d;
-    d_len2d_d_X(1) = X(1) / len2d;
-
-    const double angle_Z = std::atan2(len2d, X(2));
-    const double d_angle_Z_d_len2d = X(2) / (len2d * len2d + X(2) * X(2));
-
-    /* Ignore depth component and compute radial angle */
-    const double angle_radial = std::atan2(X(1), X(0));
-
-    Eigen::Matrix<double, 2, 3> d_angles_d_X;
-    d_angles_d_X(0, 0) = -X(1) / (X(0) * X(0) + X(1) * X(1));
-    d_angles_d_X(0, 1) = X(0) / (X(0) * X(0) + X(1) * X(1));
-    d_angles_d_X(0, 2) = 0.0;
-
-    d_angles_d_X(1, 0) = d_angle_Z_d_len2d * d_len2d_d_X(0);
-    d_angles_d_X(1, 1) = d_angle_Z_d_len2d * d_len2d_d_X(1);
-    d_angles_d_X(1, 2) = -len2d / (len2d * len2d + X(2) * X(2));
-
-    const double rsensor = std::min(sensorWidth(), sensorHeight());
-    const double rscale = sensorWidth() / std::max(w(), h());
-    const double fmm = _scale(0) * rscale;
-    const double fov = rsensor / fmm;
-
-    const double radius = angle_Z / (0.5 * fov);
-
-    const double d_radius_d_angle_Z = 1.0 / (0.5 * fov);
-
-    /* radius = focal * angle_Z */
-    const Vec2 P{cos(angle_radial) * radius, sin(angle_radial) * radius};
-
-    Eigen::Matrix<double, 2, 2> d_P_d_angles;
-    d_P_d_angles(0, 0) = -sin(angle_radial) * radius;
-    d_P_d_angles(0, 1) = cos(angle_radial) * d_radius_d_angle_Z;
-    d_P_d_angles(1, 0) = cos(angle_radial) * radius;
-    d_P_d_angles(1, 1) = sin(angle_radial) * d_radius_d_angle_Z;
-
-    return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_angles * d_angles_d_X * d_X_d_T.block<3, 16>(0, 0);
-}
-
-Eigen::Matrix<double, 2, 4> Equidistant::getDerivativeTransformProjectWrtPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const
-{
-    Eigen::Matrix4d T = pose;
-    const Vec4 X = T * pt;  // apply pose
-
-    const Eigen::Matrix4d& d_X_d_pt = T;
-
-    /* Compute angle with optical center */
-    const double len2d = sqrt(X(0) * X(0) + X(1) * X(1));
-    Eigen::Matrix<double, 2, 2> d_len2d_d_X;
-    d_len2d_d_X(0) = X(0) / len2d;
-    d_len2d_d_X(1) = X(1) / len2d;
-
-    const double angle_Z = std::atan2(len2d, X(2));
-    const double d_angle_Z_d_len2d = X(2) / (len2d * len2d + X(2) * X(2));
-
-    /* Ignore depth component and compute radial angle */
-    const double angle_radial = std::atan2(X(1), X(0));
-
-    Eigen::Matrix<double, 2, 4> d_angles_d_X;
-    d_angles_d_X(0, 0) = -X(1) / (X(0) * X(0) + X(1) * X(1));
-    d_angles_d_X(0, 1) = X(0) / (X(0) * X(0) + X(1) * X(1));
-    d_angles_d_X(0, 2) = 0.0;
-    d_angles_d_X(0, 3) = 0.0;
-
-    d_angles_d_X(1, 0) = d_angle_Z_d_len2d * d_len2d_d_X(0);
-    d_angles_d_X(1, 1) = d_angle_Z_d_len2d * d_len2d_d_X(1);
-    d_angles_d_X(1, 2) = -len2d / (len2d * len2d + X(2) * X(2));
-    d_angles_d_X(1, 3) = 0.0;
-
-    const double rsensor = std::min(sensorWidth(), sensorHeight());
-    const double rscale = sensorWidth() / std::max(w(), h());
-    const double fmm = _scale(0) * rscale;
-    const double fov = rsensor / fmm;
-    const double radius = angle_Z / (0.5 * fov);
-
-    double d_radius_d_angle_Z = 1.0 / (0.5 * fov);
-
-    /* radius = focal * angle_Z */
-    const Vec2 P{cos(angle_radial) * radius, sin(angle_radial) * radius};
-
-    Eigen::Matrix<double, 2, 2> d_P_d_angles;
-    d_P_d_angles(0, 0) = -sin(angle_radial) * radius;
-    d_P_d_angles(0, 1) = cos(angle_radial) * d_radius_d_angle_Z;
-    d_P_d_angles(1, 0) = cos(angle_radial) * radius;
-    d_P_d_angles(1, 1) = sin(angle_radial) * d_radius_d_angle_Z;
-
-    return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_angles * d_angles_d_X * d_X_d_pt;
-}
-
 Eigen::Matrix<double, 2, 3> Equidistant::getDerivativeTransformProjectWrtPoint3(const Eigen::Matrix4d& T, const Vec4& pt) const
 {
     const Vec4 X = T * pt;  // apply pose

src/aliceVision/camera/Equidistant.hpp

@@ -76,14 +76,6 @@ class Equidistant : public IntrinsicScaleOffsetDisto
 
     Vec2 project(const Vec4& pt, bool applyDistortion = true) const override;
 
-    Eigen::Matrix<double, 2, 9> getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
-
-    Eigen::Matrix<double, 2, 16> getDerivativeTransformProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
-
-    Eigen::Matrix<double, 2, 16> getDerivativeTransformProjectWrtPoseLeft(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
-
-    Eigen::Matrix<double, 2, 4> getDerivativeTransformProjectWrtPoint(const Eigen::Matrix4d& pose, const Vec4& pt) 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;