Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add Equirectangular camera support for SFM #1801

Merged
merged 7 commits into from
Jan 10, 2025
Merged
Show file tree
Hide file tree
Changes from 5 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions src/aliceVision/aliceVision.i
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
2 changes: 2 additions & 0 deletions src/aliceVision/camera/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@ set(camera_files_headers
Undistortion3DEClassicLD.hpp
UndistortionRadial.hpp
Equidistant.hpp
Equirectangular.hpp
IntrinsicBase.hpp
IntrinsicInitMode.hpp
IntrinsicScaleOffset.hpp
Expand All @@ -28,6 +29,7 @@ set(camera_files_sources
DistortionRadial.cpp
UndistortionRadial.cpp
Equidistant.cpp
Equirectangular.cpp
IntrinsicBase.cpp
IntrinsicScaleOffset.cpp
IntrinsicScaleOffsetDisto.cpp
Expand Down
1 change: 1 addition & 0 deletions src/aliceVision/camera/Camera.i
Original file line number Diff line number Diff line change
Expand Up @@ -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>
197 changes: 0 additions & 197 deletions src/aliceVision/camera/Equidistant.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
8 changes: 0 additions & 8 deletions src/aliceVision/camera/Equidistant.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Loading
Loading