diff --git a/src/aliceVision/camera/Equidistant.cpp b/src/aliceVision/camera/Equidistant.cpp index ced2f085c9..6af2b3d2c0 100644 --- a/src/aliceVision/camera/Equidistant.cpp +++ b/src/aliceVision/camera/Equidistant.cpp @@ -67,203 +67,6 @@ Vec2 Equidistant::project(const Vec4& pt, bool applyDistortion) const return pt_ima; } -Eigen::Matrix Equidistant::getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const -{ - Eigen::Matrix4d T = pose; - const Vec4 X = T * pt; // apply pose - - const Eigen::Matrix 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 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 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 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 Equidistant::getDerivativeTransformProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt) const -{ - Eigen::Matrix4d T = pose; - const Vec4 X = T * pt; // apply pose - - const Eigen::Matrix 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 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 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 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 Equidistant::getDerivativeTransformProjectWrtPoseLeft(const Eigen::Matrix4d& pose, const Vec4& pt) const -{ - Eigen::Matrix4d T = pose; - const Vec4 X = T * pt; // apply pose - - const Eigen::Matrix 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 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 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 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 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 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 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 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 Equidistant::getDerivativeTransformProjectWrtPoint3(const Eigen::Matrix4d& T, const Vec4& pt) const { const Vec4 X = T * pt; // apply pose diff --git a/src/aliceVision/camera/Equidistant.hpp b/src/aliceVision/camera/Equidistant.hpp index b903e90d7e..0fd4806118 100644 --- a/src/aliceVision/camera/Equidistant.hpp +++ b/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 getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const override; - - Eigen::Matrix getDerivativeTransformProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt) const override; - - Eigen::Matrix getDerivativeTransformProjectWrtPoseLeft(const Eigen::Matrix4d& pose, const Vec4& pt) const override; - - Eigen::Matrix getDerivativeTransformProjectWrtPoint(const Eigen::Matrix4d& pose, const Vec4& pt) 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; diff --git a/src/aliceVision/camera/IntrinsicBase.hpp b/src/aliceVision/camera/IntrinsicBase.hpp index e27c3ee138..e8712df49b 100644 --- a/src/aliceVision/camera/IntrinsicBase.hpp +++ b/src/aliceVision/camera/IntrinsicBase.hpp @@ -136,38 +136,7 @@ class IntrinsicBase Eigen::Matrix getDerivativeCartesianfromSphericalCoordinates(const Vec3& pt); - /** - * @brief Get the derivative of a projection of a 3D point into the camera plane - * @param[in] pose The pose - * @param[in] pt3D The 3D point - * @return The projection jacobian with respect to the pose - */ - virtual Eigen::Matrix getDerivativeTransformProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0; - - /** - * @brief Get the derivative of a projection of a 3D point into the camera plane - * @param[in] pose The pose - * @param[in] pt3D The 3D point - * @return The projection jacobian with respect to the rotation - */ - virtual Eigen::Matrix getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const = 0; - - /** - * @brief Get the derivative of a projection of a 3D point into the camera plane - * @param[in] pose The pose - * @param[in] pt3D The 3D point - * @return The projection jacobian with respect to the pose - */ - virtual Eigen::Matrix getDerivativeTransformProjectWrtPoseLeft(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0; - - /** - * @brief Get the derivative of a projection of a 3D point into the camera plane - * @param[in] pose The pose - * @param[in] pt3D The 3D point - * @return The projection jacobian with respect to the point - */ - virtual Eigen::Matrix getDerivativeTransformProjectWrtPoint(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0; - + /** * @brief Get the derivative of a projection of a 3D point into the camera plane * @param[in] pose The pose diff --git a/src/aliceVision/camera/Pinhole.cpp b/src/aliceVision/camera/Pinhole.cpp index c9c66a15f2..c99cce4a0b 100644 --- a/src/aliceVision/camera/Pinhole.cpp +++ b/src/aliceVision/camera/Pinhole.cpp @@ -61,132 +61,6 @@ Vec2 Pinhole::project(const Vec4& pt, bool applyDistortion) const return impt; } -Eigen::Matrix Pinhole::getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const -{ - const Vec4 X = pose * pt; // apply pose - - const Eigen::Matrix d_X_d_R = getJacobian_AB_wrt_A<3, 3, 1>(pose.block<3, 3>(0, 0), pt.head(3)); - - const Vec2 P = X.head<2>() / X(2); - - Eigen::Matrix d_P_d_X; - d_P_d_X(0, 0) = 1 / X(2); - d_P_d_X(0, 1) = 0; - d_P_d_X(0, 2) = -X(0) / (X(2) * X(2)); - d_P_d_X(1, 0) = 0; - d_P_d_X(1, 1) = 1 / X(2); - d_P_d_X(1, 2) = -X(1) / (X(2) * X(2)); - - return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_X * d_X_d_R; -} - -Eigen::Matrix Pinhole::getDerivativeTransformProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt) const -{ - const Eigen::Matrix4d T = pose; - - const Vec4 X = T * pt; // apply pose - - const Eigen::Matrix d_X_d_T = getJacobian_AB_wrt_A<4, 4, 1>(T, pt); - - const Vec2 P = X.head<2>() / X(2); - - Eigen::Matrix d_P_d_X; - d_P_d_X(0, 0) = 1 / X(2); - d_P_d_X(0, 1) = 0; - d_P_d_X(0, 2) = -X(0) / (X(2) * X(2)); - d_P_d_X(1, 0) = 0; - d_P_d_X(1, 1) = 1 / X(2); - d_P_d_X(1, 2) = -X(1) / (X(2) * X(2)); - - return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_X * d_X_d_T.block<3, 16>(0, 0); -} - -Eigen::Matrix Pinhole::getDerivativeTransformProjectWrtPoseLeft(const Eigen::Matrix4d& pose, const Vec4& pt) const -{ - const Eigen::Matrix4d T = pose; - - const Vec4 X = T * pt; // apply pose - - const Eigen::Matrix d_X_d_T = getJacobian_AB_wrt_A<4, 4, 1>(Eigen::Matrix4d::Identity(), X); - - const Vec2 P = X.head<2>() / X(2); - - double x = X(0); - double y = X(1); - double z = X(2); - double invz = 1.0 / z; - double invzsq = invz * invz; - double mxinvzsq = -x * invzsq; - double myinvzsq = -y * invzsq; - - Eigen::Matrix d_P_d_X; - d_P_d_X(0, 0) = 1 / X(2); - d_P_d_X(0, 1) = 0; - d_P_d_X(0, 2) = -X(0) / (X(2) * X(2)); - d_P_d_X(1, 0) = 0; - d_P_d_X(1, 1) = 1 / X(2); - d_P_d_X(1, 2) = -X(1) / (X(2) * X(2)); - - Eigen::Matrix d_P_d_T; - d_P_d_T(0, 0) = invz * X(0); - d_P_d_T(0, 1) = 0.0; - d_P_d_T(0, 2) = mxinvzsq * X(0); - d_P_d_T(0, 3) = 0; - d_P_d_T(0, 4) = invz * X(1); - d_P_d_T(0, 5) = 0.0; - d_P_d_T(0, 6) = mxinvzsq * X(1); - d_P_d_T(0, 7) = 0; - d_P_d_T(0, 8) = invz * X(2); - d_P_d_T(0, 9) = 0.0; - d_P_d_T(0, 10) = mxinvzsq * X(2); - d_P_d_T(0, 11) = 0; - d_P_d_T(0, 12) = invz; - d_P_d_T(0, 13) = 0.0; - d_P_d_T(0, 14) = mxinvzsq; - d_P_d_T(0, 15) = 0; - - d_P_d_T(1, 0) = 0.0; - d_P_d_T(1, 1) = invz * X(0); - d_P_d_T(1, 2) = myinvzsq * X(0); - d_P_d_T(1, 3) = 0.0; - d_P_d_T(1, 4) = 0.0; - d_P_d_T(1, 5) = invz * X(1); - d_P_d_T(1, 6) = myinvzsq * X(1); - d_P_d_T(1, 7) = 0.0; - d_P_d_T(1, 8) = 0.0; - d_P_d_T(1, 9) = invz * X(2); - d_P_d_T(1, 10) = myinvzsq * X(2); - d_P_d_T(1, 11) = 0.0; - d_P_d_T(1, 12) = 0.0; - d_P_d_T(1, 13) = invz; - d_P_d_T(1, 14) = myinvzsq; - d_P_d_T(1, 15) = 0.0; - - return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_T; -} - -Eigen::Matrix Pinhole::getDerivativeTransformProjectWrtPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const -{ - const Eigen::Matrix4d T = pose; - const Vec4 X = T * pt; // apply pose - - const Eigen::Matrix& d_X_d_P = getJacobian_AB_wrt_B<4, 4, 1>(T, pt); - - const Vec2 P = X.head<2>() / X(2); - - Eigen::Matrix d_P_d_X; - d_P_d_X(0, 0) = 1 / X(2); - d_P_d_X(0, 1) = 0; - d_P_d_X(0, 2) = -X(0) / (X(2) * X(2)); - d_P_d_X(0, 3) = 0; - d_P_d_X(1, 0) = 0; - d_P_d_X(1, 1) = 1 / X(2); - d_P_d_X(1, 2) = -X(1) / (X(2) * X(2)); - d_P_d_X(1, 3) = 0; - - return getDerivativeCam2ImaWrtPoint() * getDerivativeAddDistoWrtPt(P) * d_P_d_X * d_X_d_P; -} - Eigen::Matrix Pinhole::getDerivativeTransformProjectWrtPoint3(const Eigen::Matrix4d& T, const Vec4& pt) const { const Vec4 X = T * pt; // apply pose diff --git a/src/aliceVision/camera/Pinhole.hpp b/src/aliceVision/camera/Pinhole.hpp index 257e51c984..f0c7a39a0e 100644 --- a/src/aliceVision/camera/Pinhole.hpp +++ b/src/aliceVision/camera/Pinhole.hpp @@ -74,13 +74,6 @@ class Pinhole : public IntrinsicScaleOffsetDisto Vec2 project(const Vec4& pt, bool applyDistortion = true) const override; - Eigen::Matrix getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const override; - - Eigen::Matrix getDerivativeTransformProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt) const override; - - Eigen::Matrix getDerivativeTransformProjectWrtPoseLeft(const Eigen::Matrix4d& pose, const Vec4& pt) const override; - - Eigen::Matrix getDerivativeTransformProjectWrtPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const override; Eigen::Matrix getDerivativeTransformProjectWrtPoint3(const Eigen::Matrix4d& pose, const Vec4& pt) const override; diff --git a/src/aliceVision/sfm/bundle/costfunctions/panoramaEquidistant.hpp b/src/aliceVision/sfm/bundle/costfunctions/panoramaEquidistant.hpp deleted file mode 100644 index 6d553b1c4a..0000000000 --- a/src/aliceVision/sfm/bundle/costfunctions/panoramaEquidistant.hpp +++ /dev/null @@ -1,122 +0,0 @@ -// Copyright (c) 2023 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 -#include -#include -#include - -namespace aliceVision { -namespace sfm { - -class CostPanoramaEquidistant : public ceres::SizedCostFunction<2, 16, 16, 7> -{ - public: - CostPanoramaEquidistant(Vec2 fi, Vec2 fj, std::shared_ptr& intrinsic) - : _fi(fi), - _fj(fj), - _intrinsic(intrinsic) - {} - - bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override - { - Vec2 pt_i = _fi; - Vec2 pt_j = _fj; - - const double* parameter_pose_i = parameters[0]; - const double* parameter_pose_j = parameters[1]; - const double* parameter_intrinsics = parameters[2]; - - const Eigen::Map> iTo(parameter_pose_i); - const Eigen::Map> jTo(parameter_pose_j); - - Eigen::Matrix iRo = iTo.block<3, 3>(0, 0); - Eigen::Matrix jRo = jTo.block<3, 3>(0, 0); - - _intrinsic->setScale({parameter_intrinsics[0], parameter_intrinsics[1]}); - _intrinsic->setOffset({parameter_intrinsics[2], parameter_intrinsics[3]}); - _intrinsic->setDistortionParamsFn(3, [&](auto index) { return parameter_intrinsics[4 + index]; }); - - Eigen::Matrix3d R = jRo * iRo.transpose(); - - geometry::Pose3 T_pose3(R, Vec3({0, 0, 0})); - Eigen::Matrix4d T = T_pose3.getHomogeneous(); - - Vec2 pt_i_cam = _intrinsic->ima2cam(pt_i); - Vec2 pt_i_undist = _intrinsic->removeDistortion(pt_i_cam); - Vec4 pt_i_sphere = _intrinsic->toUnitSphere(pt_i_undist).homogeneous(); - - Vec2 pt_j_est = _intrinsic->transformProject(T_pose3, pt_i_sphere, true); - - residuals[0] = pt_j_est(0) - pt_j(0); - residuals[1] = pt_j_est(1) - pt_j(1); - - if (jacobians == nullptr) - { - return true; - } - - if (jacobians[0] != nullptr) - { - Eigen::Map> J(jacobians[0]); - - Eigen::Matrix J9 = _intrinsic->getDerivativeTransformProjectWrtRotation(T, pt_i_sphere) * - getJacobian_AB_wrt_B<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_At_wrt_A<3, 3>() * - getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), iRo); - - J.fill(0); - J.block<2, 3>(0, 0) = J9.block<2, 3>(0, 0); - J.block<2, 3>(0, 4) = J9.block<2, 3>(0, 3); - J.block<2, 3>(0, 8) = J9.block<2, 3>(0, 6); - } - - if (jacobians[1] != nullptr) - { - Eigen::Map> J(jacobians[1]); - - Eigen::Matrix J9 = _intrinsic->getDerivativeTransformProjectWrtRotation(T, pt_i_sphere) * - getJacobian_AB_wrt_A<3, 3, 3>(jRo, iRo.transpose()) * - getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), jRo); - - J.fill(0); - J.block<2, 3>(0, 0) = J9.block<2, 3>(0, 0); - J.block<2, 3>(0, 4) = J9.block<2, 3>(0, 3); - J.block<2, 3>(0, 8) = J9.block<2, 3>(0, 6); - } - - if (jacobians[2] != nullptr) - { - Eigen::Map> J(jacobians[2]); - - Eigen::Matrix Jhomogenous = Eigen::Matrix::Identity(); - - Eigen::Matrix Jscale = - _intrinsic->getDerivativeTransformProjectWrtScale(T, pt_i_sphere) + - _intrinsic->getDerivativeTransformProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * _intrinsic->getDerivativetoUnitSphereWrtScale(pt_i_undist); - Eigen::Matrix Jpp = - _intrinsic->getDerivativeTransformProjectWrtPrincipalPoint(T, pt_i_sphere) + - _intrinsic->getDerivativeTransformProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * _intrinsic->getDerivativetoUnitSphereWrtPoint(pt_i_undist) * - _intrinsic->getDerivativeRemoveDistoWrtPt(pt_i_cam) * _intrinsic->getDerivativeIma2CamWrtPrincipalPoint(); - Eigen::Matrix Jdisto = - _intrinsic->getDerivativeTransformProjectWrtDisto(T, pt_i_sphere) + _intrinsic->getDerivativeTransformProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * - _intrinsic->getDerivativetoUnitSphereWrtPoint(pt_i_undist) * - _intrinsic->getDerivativeRemoveDistoWrtDisto(pt_i_cam); - - J.block<2, 2>(0, 0) = Jscale; - J.block<2, 2>(0, 2) = Jpp; - J.block<2, 3>(0, 4) = Jdisto; - } - - return true; - } - - private: - Vec2 _fi; - Vec2 _fj; - std::shared_ptr _intrinsic; -}; - -} // namespace sfm -} // namespace aliceVision \ No newline at end of file diff --git a/src/aliceVision/sfm/bundle/costfunctions/panoramaPinhole.hpp b/src/aliceVision/sfm/bundle/costfunctions/panoramaPinhole.hpp deleted file mode 100644 index 823fe7bcd1..0000000000 --- a/src/aliceVision/sfm/bundle/costfunctions/panoramaPinhole.hpp +++ /dev/null @@ -1,137 +0,0 @@ -// Copyright (c) 2023 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 -#include -#include -#include - -namespace aliceVision { -namespace sfm { - -class CostPanoramaPinHole : public ceres::CostFunction -{ - public: - CostPanoramaPinHole(Vec2 fi, Vec2 fj, std::shared_ptr& intrinsic) - : _fi(fi), - _fj(fj), - _intrinsic(intrinsic) - { - set_num_residuals(2); - - mutable_parameter_block_sizes()->push_back(16); - mutable_parameter_block_sizes()->push_back(16); - mutable_parameter_block_sizes()->push_back(intrinsic->getParams().size()); - } - - bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override - { - Vec2 pt_i = _fi; - Vec2 pt_j = _fj; - - const double* parameter_pose_i = parameters[0]; - const double* parameter_pose_j = parameters[1]; - const double* parameter_intrinsics = parameters[2]; - - const Eigen::Map> iTo(parameter_pose_i); - const Eigen::Map> jTo(parameter_pose_j); - - Eigen::Matrix iRo = iTo.block<3, 3>(0, 0); - Eigen::Matrix jRo = jTo.block<3, 3>(0, 0); - - _intrinsic->setScale({parameter_intrinsics[0], parameter_intrinsics[1]}); - _intrinsic->setOffset({parameter_intrinsics[2], parameter_intrinsics[3]}); - - size_t params_size = _intrinsic->getParamsSize(); - size_t disto_size = _intrinsic->getDistortionParamsSize(); - size_t offset = params_size - disto_size; - - _intrinsic->setDistortionParamsFn(disto_size, [&](auto index) { return parameter_intrinsics[offset + index]; }); - - Eigen::Matrix3d R = jRo * iRo.transpose(); - geometry::Pose3 T_pose3(R, Vec3({0, 0, 0})); - Eigen::Matrix4d T = T_pose3.getHomogeneous(); - - Vec2 pt_i_cam = _intrinsic->ima2cam(pt_i); - Vec2 pt_i_undist = _intrinsic->removeDistortion(pt_i_cam); - Vec4 pt_i_sphere = _intrinsic->toUnitSphere(pt_i_undist).homogeneous(); - - Vec2 pt_j_est = _intrinsic->transformProject(T_pose3, pt_i_sphere, true); - - residuals[0] = pt_j_est(0) - pt_j(0); - residuals[1] = pt_j_est(1) - pt_j(1); - - if (jacobians == nullptr) - { - return true; - } - - if (jacobians[0] != nullptr) - { - Eigen::Map> J(jacobians[0]); - - Eigen::Matrix J9 = _intrinsic->getDerivativeTransformProjectWrtRotation(T, pt_i_sphere) * - getJacobian_AB_wrt_B<3, 3, 3>(jRo, iRo.transpose()) * getJacobian_At_wrt_A<3, 3>() * - getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), iRo); - - J.fill(0); - J.block<2, 3>(0, 0) = J9.block<2, 3>(0, 0); - J.block<2, 3>(0, 4) = J9.block<2, 3>(0, 3); - J.block<2, 3>(0, 8) = J9.block<2, 3>(0, 6); - } - - if (jacobians[1] != nullptr) - { - Eigen::Map> J(jacobians[1]); - - Eigen::Matrix J9 = _intrinsic->getDerivativeTransformProjectWrtRotation(T, pt_i_sphere) * - getJacobian_AB_wrt_A<3, 3, 3>(jRo, iRo.transpose()) * - getJacobian_AB_wrt_A<3, 3, 3>(Eigen::Matrix3d::Identity(), jRo); - - J.fill(0); - J.block<2, 3>(0, 0) = J9.block<2, 3>(0, 0); - J.block<2, 3>(0, 4) = J9.block<2, 3>(0, 3); - J.block<2, 3>(0, 8) = J9.block<2, 3>(0, 6); - } - - if (jacobians[2] != nullptr) - { - Eigen::Map> J(jacobians[2], 2, params_size); - - Eigen::Matrix Jhomogenous = Eigen::Matrix::Identity(); - - Eigen::Matrix Jscale = - _intrinsic->getDerivativeTransformProjectWrtScale(T, pt_i_sphere) + - _intrinsic->getDerivativeTransformProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * _intrinsic->getDerivativetoUnitSphereWrtPoint(pt_i_undist) * - _intrinsic->getDerivativeRemoveDistoWrtPt(pt_i_cam) * _intrinsic->getDerivativeIma2CamWrtScale(pt_i); - Eigen::Matrix Jpp = - _intrinsic->getDerivativeTransformProjectWrtPrincipalPoint(T, pt_i_sphere) + - _intrinsic->getDerivativeTransformProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * _intrinsic->getDerivativetoUnitSphereWrtPoint(pt_i_undist) * - _intrinsic->getDerivativeRemoveDistoWrtPt(pt_i_cam) * _intrinsic->getDerivativeIma2CamWrtPrincipalPoint(); - - J.block<2, 2>(0, 0) = Jscale; - J.block<2, 2>(0, 2) = Jpp; - - if (disto_size > 0) - { - Eigen::Matrix Jdisto = - _intrinsic->getDerivativeTransformProjectWrtDisto(T, pt_i_sphere) + _intrinsic->getDerivativeTransformProjectWrtPoint(T, pt_i_sphere) * Jhomogenous * - _intrinsic->getDerivativetoUnitSphereWrtPoint(pt_i_undist) * - _intrinsic->getDerivativeRemoveDistoWrtDisto(pt_i_cam); - J.block(0, 4, 2, disto_size) = Jdisto; - } - } - - return true; - } - - private: - Vec2 _fi; - Vec2 _fj; - std::shared_ptr _intrinsic; -}; - -} // namespace sfm -} // namespace aliceVision \ No newline at end of file diff --git a/src/aliceVision/sfm/bundle/costfunctions/projectionSimple.hpp b/src/aliceVision/sfm/bundle/costfunctions/projectionSimple.hpp deleted file mode 100644 index 1a43564c10..0000000000 --- a/src/aliceVision/sfm/bundle/costfunctions/projectionSimple.hpp +++ /dev/null @@ -1,87 +0,0 @@ -// Copyright (c) 2023 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 -#include -#include -#include - -namespace aliceVision { -namespace sfm { - -class CostProjectionSimple : public ceres::CostFunction -{ - public: - CostProjectionSimple(const sfmData::Observation& measured, const std::shared_ptr& intrinsics) - : _measured(measured), - _intrinsics(intrinsics) - { - set_num_residuals(2); - - mutable_parameter_block_sizes()->push_back(16); - mutable_parameter_block_sizes()->push_back(intrinsics->getParams().size()); - mutable_parameter_block_sizes()->push_back(3); - } - - bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override - { - const double* parameter_pose = parameters[0]; - const double* parameter_intrinsics = parameters[1]; - const double* parameter_landmark = parameters[2]; - - const Eigen::Map T(parameter_pose); - const Eigen::Map pt(parameter_landmark); - - const Vec4 pth = pt.homogeneous(); - - const Vec4 cpt = T * pth; - - Vec2 pt_est = _intrinsics->transformProject(T, pth, true); - const double scale = (_measured.getScale() > 1e-12) ? _measured.getScale() : 1.0; - - residuals[0] = (pt_est(0) - _measured.getX()) / scale; - residuals[1] = (pt_est(1) - _measured.getY()) / scale; - - if (jacobians == nullptr) - { - return true; - } - - const geometry::Pose3 T_pose3(T); - size_t params_size = _intrinsics->getParams().size(); - - double d_res_d_pt_est = 1.0 / scale; - - if (jacobians[0] != nullptr) - { - Eigen::Map> J(jacobians[0]); - - J = d_res_d_pt_est * _intrinsics->getDerivativeTransformProjectWrtPoseLeft(T, pth); - } - - if (jacobians[1] != nullptr) - { - Eigen::Map> J(jacobians[1], 2, params_size); - - J = d_res_d_pt_est * _intrinsics->getDerivativeTransformProjectWrtParams(T, pth); - } - - if (jacobians[2] != nullptr) - { - Eigen::Map> J(jacobians[2]); - - J = d_res_d_pt_est * _intrinsics->getDerivativeTransformProjectWrtPoint3(T, pth); - } - - return true; - } - - private: - const sfmData::Observation& _measured; - const std::shared_ptr _intrinsics; -}; - -} // namespace sfm -} // namespace aliceVision \ No newline at end of file