Skip to content

Commit

Permalink
Remove useless files and functions
Browse files Browse the repository at this point in the history
  • Loading branch information
@servantftechnicolor
servantftechnicolor committed Jan 9, 2025
1 parent ba96d10 commit 44add21
Show file tree
Hide file tree
Showing 8 changed files with 1 addition and 716 deletions.
197 changes: 0 additions & 197 deletions src/aliceVision/camera/Equidistant.cpp
View file
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
View file
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
33 changes: 1 addition & 32 deletions src/aliceVision/camera/IntrinsicBase.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -136,38 +136,7 @@ class IntrinsicBase

Eigen::Matrix<double, 4, 3> 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<double, 2, 16> 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<double, 2, 9> 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<double, 2, 16> 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<double, 2, 4> 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
Expand Down
126 changes: 0 additions & 126 deletions src/aliceVision/camera/Pinhole.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,132 +61,6 @@ Vec2 Pinhole::project(const Vec4& pt, bool applyDistortion) const
return impt;
}

Eigen::Matrix<double, 2, 9> Pinhole::getDerivativeTransformProjectWrtRotation(const Eigen::Matrix4d& pose, const Vec4& pt) const
{
const Vec4 X = pose * 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));

const Vec2 P = X.head<2>() / X(2);

Eigen::Matrix<double, 2, 3> 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<double, 2, 16> 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<double, 4, 16> d_X_d_T = getJacobian_AB_wrt_A<4, 4, 1>(T, pt);

const Vec2 P = X.head<2>() / X(2);

Eigen::Matrix<double, 2, 3> 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<double, 2, 16> 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<double, 4, 16> 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<double, 2, 3> 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<double, 2, 16> 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<double, 2, 4> 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<double, 4, 4>& d_X_d_P = getJacobian_AB_wrt_B<4, 4, 1>(T, pt);

const Vec2 P = X.head<2>() / X(2);

Eigen::Matrix<double, 2, 4> 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<double, 2, 3> Pinhole::getDerivativeTransformProjectWrtPoint3(const Eigen::Matrix4d& T, const Vec4& pt) const
{
const Vec4 X = T * pt; // apply pose
Expand Down
Loading

0 comments on commit 44add21

Please sign in to comment.