From 0b1ce378cc66c9e7aacfd0a7b8027d44771992c5 Mon Sep 17 00:00:00 2001 From: Maurizio Monge Date: Sun, 9 Nov 2025 06:27:33 -0800 Subject: [PATCH] {Feature} Calib jacobians (#304) Summary: Add Jacobians wrt the calibration parameters. This is required to make VI-BA work. This diff also changes some return types to const references, as it is better practice to avoid creating temporary objects with dynamic allocation, and allows Mut fields so that the CameraCalibration type can be used in optimization. Differential Revision: D85353427 Pulled By: maurimo --- core/calibration/CameraCalibration.cpp | 42 ++++++- core/calibration/CameraCalibration.h | 50 +++++++- .../camera_projections/CMakeLists.txt | 1 + .../camera_projections/CameraProjection.cpp | 66 ++++++++--- .../camera_projections/CameraProjection.h | 46 ++++++-- .../FisheyeRadTanThinPrism.h | 110 +++++++++++++----- .../camera_projections/KannalaBrandtK3.h | 60 +++++++++- core/calibration/camera_projections/Linear.h | 33 ++++-- .../camera_projections/Spherical.h | 11 +- .../test/CameraProjectionTest.cpp | 24 +++- core/calibration/utility/CMakeLists.txt | 5 + core/calibration/utility/NullRef.h | 78 +++++++++++++ core/python/DeviceCalibrationPyBind.h | 57 +++++++-- 13 files changed, 498 insertions(+), 85 deletions(-) create mode 100644 core/calibration/utility/NullRef.h diff --git a/core/calibration/CameraCalibration.cpp b/core/calibration/CameraCalibration.cpp index a0f03d9e5..e273f3e2e 100644 --- a/core/calibration/CameraCalibration.cpp +++ b/core/calibration/CameraCalibration.cpp @@ -30,7 +30,7 @@ CameraCalibration::CameraCalibration( const double maxSolidAngle, const std::string& serialNumber, const double timeOffsetSecDeviceCamera, - const std::optional maybeReadOutTimeSec) + const std::optional& maybeReadOutTimeSec) : label_(label), projectionModel_(projectionModelType, projectionParams), T_Device_Camera_(T_Device_Camera), @@ -70,10 +70,18 @@ double CameraCalibration::getTimeOffsetSecDeviceCamera() const { return timeOffsetSecDeviceCamera_; } +double& CameraCalibration::getTimeOffsetSecDeviceCameraMut() { + return timeOffsetSecDeviceCamera_; +} + std::optional CameraCalibration::getReadOutTimeSec() const { return maybeReadOutTimeSec_; } +std::optional& CameraCalibration::getReadOutTimeSecMut() { + return maybeReadOutTimeSec_; +} + namespace { // A helper function to determine if a pixel is within valid mask in a camera inline bool checkPixelValidInMask( @@ -110,10 +118,26 @@ CameraProjection::ModelType CameraCalibration::modelName() const { return projectionModel_.modelName(); } -Eigen::VectorXd CameraCalibration::projectionParams() const { +const Eigen::VectorXd& CameraCalibration::projectionParams() const { return projectionModel_.projectionParams(); } +Eigen::VectorXd& CameraCalibration::projectionParamsMut() { + return projectionModel_.projectionParamsMut(); +} + +int CameraCalibration::numParameters() const { + return projectionModel_.numParameters(); +} + +int CameraCalibration::numProjectionParameters() const { + return projectionModel_.numProjectionParameters(); +} + +int CameraCalibration::numDistortionParameters() const { + return projectionModel_.numDistortionParameters(); +} + Eigen::Vector2d CameraCalibration::getPrincipalPoint() const { return projectionModel_.getPrincipalPoint(); } @@ -122,15 +146,21 @@ Eigen::Vector2d CameraCalibration::getFocalLengths() const { return projectionModel_.getFocalLengths(); } -Eigen::Vector2d CameraCalibration::projectNoChecks(const Eigen::Vector3d& pointInCamera) const { - return projectionModel_.project(pointInCamera); +Eigen::Vector2d CameraCalibration::projectNoChecks( + const Eigen::Vector3d& pointInCamera, + Eigen::Ref> jacobianWrtPoint, + Eigen::Ref> jacobianWrtParams) const { + return projectionModel_.project(pointInCamera, jacobianWrtPoint, jacobianWrtParams); } std::optional CameraCalibration::project( - const Eigen::Vector3d& pointInCamera) const { + const Eigen::Vector3d& pointInCamera, + Eigen::Ref> jacobianWrtPoint, + Eigen::Ref> jacobianWrtParams) const { // check point is in front of camera if (checkPointInVisibleCone(pointInCamera, maxSolidAngle_)) { - const Eigen::Vector2d cameraPixel = projectNoChecks(pointInCamera); + const Eigen::Vector2d cameraPixel = + projectNoChecks(pointInCamera, jacobianWrtPoint, jacobianWrtParams); if (isVisible(cameraPixel)) { return cameraPixel; } diff --git a/core/calibration/CameraCalibration.h b/core/calibration/CameraCalibration.h index 3455e0a68..404636820 100644 --- a/core/calibration/CameraCalibration.h +++ b/core/calibration/CameraCalibration.h @@ -37,6 +37,26 @@ class CameraCalibration { */ CameraCalibration() = default; + /** + * @brief Default copy constructor. + */ + CameraCalibration(const CameraCalibration&) = default; + + /** + * @brief Default move constructor. + */ + CameraCalibration(CameraCalibration&&) = default; + + /** + * @brief Default copy assignment. + */ + CameraCalibration& operator=(const CameraCalibration&) = default; + + /** + * @brief Default move assignment. + */ + CameraCalibration& operator=(CameraCalibration&&) = default; + /** * @brief Constructor with a list of parameters for CameraCalibration. * @param label The label of the camera, e.g. "camera-slam-left". @@ -67,7 +87,7 @@ class CameraCalibration { double maxSolidAngle, const std::string& serialNumber = "", double timeOffsetSecDeviceCamera = 0.0, - std::optional maybeReadOutTimeSec = {}); + const std::optional& maybeReadOutTimeSec = {}); std::string getLabel() const; std::string getSerialNumber() const; @@ -76,7 +96,9 @@ class CameraCalibration { double getMaxSolidAngle() const; std::optional getValidRadius() const; double getTimeOffsetSecDeviceCamera() const; + double& getTimeOffsetSecDeviceCameraMut(); std::optional getReadOutTimeSec() const; + std::optional& getReadOutTimeSecMut(); /** * @brief Function to check whether a pixel is within the valid area of the sensor plane. */ @@ -85,22 +107,41 @@ class CameraCalibration { CameraProjection::ModelType modelName() const; // return KB3 or Fisheye624 Eigen::Vector2d getPrincipalPoint() const; // return optical center Eigen::Vector2d getFocalLengths() const; // return focal length in x and y - Eigen::VectorXd projectionParams() const; // return full calibration parameters + const Eigen::VectorXd& projectionParams() const; // return full calibration parameters + Eigen::VectorXd& projectionParamsMut(); + int numParameters() const; // return number of parameters + int numProjectionParameters() const; // return number of projection parameters + int numDistortionParameters() const; // return number of distortion parameters /** * @brief Function to project a 3d point (in camera frame) to a 2d camera pixel location. In this * function, no check is performed. * @param pointInCamera 3d point in camera frame. + * @param jacobianWrtPoint Optional, if not null, will store the Jacobian of the projection + * with respect to the point in camera space. + * @param jacobianWrtParams Optional, if not null, will store the Jacobian of the projection + * with respect to the projection parameters. * @return 2d pixel location in image plane. */ - Eigen::Vector2d projectNoChecks(const Eigen::Vector3d& pointInCamera) const; + Eigen::Vector2d projectNoChecks( + const Eigen::Vector3d& pointInCamera, + Eigen::Ref> jacobianWrtPoint = NullRef(), + Eigen::Ref> jacobianWrtParams = NullRef()) const; + /** * @brief Function to project a 3d point (in camera frame) to a 2d camera pixel location, with a * number of validity checks to ensure the point is visible. * @param pointInCamera 3d point in camera frame. + * @param jacobianWrtPoint Optional, if not null, will store the Jacobian of the projection + * with respect to the point in camera space. + * @param jacobianWrtParams Optional, if not null, will store the Jacobian of the projection + * with respect to the projection parameters. * @return 2d pixel location in image plane. If any of the check failed, return std::nullopt. */ - std::optional project(const Eigen::Vector3d& pointInCamera) const; + std::optional project( + const Eigen::Vector3d& pointInCamera, + Eigen::Ref> jacobianWrtPoint = NullRef(), + Eigen::Ref> jacobianWrtParams = NullRef()) const; /** * @brief Function to unproject a 2d pixel location to a 3d ray in camera frame. In this function, @@ -109,6 +150,7 @@ class CameraCalibration { * @return 3d ray, in camera frame. */ Eigen::Vector3d unprojectNoChecks(const Eigen::Vector2d& cameraPixel) const; + /** * @brief Function to unproject a 2d pixel location to a 3d ray, in camera frame, with a number of * validity checks to ensure the unprojection is valid. diff --git a/core/calibration/camera_projections/CMakeLists.txt b/core/calibration/camera_projections/CMakeLists.txt index 70b0f60c8..c008562c3 100644 --- a/core/calibration/camera_projections/CMakeLists.txt +++ b/core/calibration/camera_projections/CMakeLists.txt @@ -27,5 +27,6 @@ target_link_libraries(camera_models INTERFACE common) target_include_directories(camera_models INTERFACE $) add_library(camera_projection CameraProjection.cpp CameraProjection.h CameraProjectionFormat.h) +target_link_libraries(camera_projection INTERFACE calibration_nullref) target_include_directories(camera_projection PUBLIC $) target_link_libraries(camera_projection PUBLIC camera_models format Eigen3::Eigen Sophus::Sophus) diff --git a/core/calibration/camera_projections/CameraProjection.cpp b/core/calibration/camera_projections/CameraProjection.cpp index 42ac6e0dd..b75f93ebb 100644 --- a/core/calibration/camera_projections/CameraProjection.cpp +++ b/core/calibration/camera_projections/CameraProjection.cpp @@ -39,7 +39,7 @@ typename CameraProjectionTemplated::ProjectionVariant getProjectionVaria template CameraProjectionTemplated::CameraProjectionTemplated( const ModelType& type, - const Eigen::Matrix& projectionParams) + const Eigen::Vector& projectionParams) : modelName_(type), projectionParams_(projectionParams), projectionVariant_(getProjectionVariant(type)) {} @@ -51,15 +51,50 @@ typename CameraProjectionTemplated::ModelType CameraProjectionTemplated< } template -Eigen::Matrix CameraProjectionTemplated::projectionParams() +const Eigen::Vector& CameraProjectionTemplated::projectionParams() const { return projectionParams_; } template -Eigen::Matrix CameraProjectionTemplated::getFocalLengths() const { +Eigen::Vector& CameraProjectionTemplated::projectionParamsMut() { + return projectionParams_; +} + +template +int CameraProjectionTemplated::numParameters() const { + return std::visit( + [](auto&& projection) -> int { + using T = std::decay_t; + return T::kNumParams; + }, + projectionVariant_); +} + +template +int CameraProjectionTemplated::numProjectionParameters() const { + return std::visit( + [](auto&& projection) -> int { + using T = std::decay_t; + return T::kNumParams - T::kNumDistortionParams; + }, + projectionVariant_); +} + +template +int CameraProjectionTemplated::numDistortionParameters() const { + return std::visit( + [](auto&& projection) -> int { + using T = std::decay_t; + return T::kNumDistortionParams; + }, + projectionVariant_); +} + +template +Eigen::Vector CameraProjectionTemplated::getFocalLengths() const { return std::visit( - [this](auto&& projection) -> Eigen::Matrix { + [this](auto&& projection) -> Eigen::Vector { using T = std::decay_t; int focalXIdx = T::kFocalXIdx; int focalYIdx = T::kFocalYIdx; @@ -69,9 +104,9 @@ Eigen::Matrix CameraProjectionTemplated::getFocalLengths() } template -Eigen::Matrix CameraProjectionTemplated::getPrincipalPoint() const { +Eigen::Vector CameraProjectionTemplated::getPrincipalPoint() const { return std::visit( - [this](auto&& projection) -> Eigen::Matrix { + [this](auto&& projection) -> Eigen::Vector { using T = std::decay_t; int principalPointColIdx = T::kPrincipalPointColIdx; int principalPointRowIdx = T::kPrincipalPointRowIdx; @@ -81,20 +116,25 @@ Eigen::Matrix CameraProjectionTemplated::getPrincipalPoint } template -Eigen::Matrix CameraProjectionTemplated::project( - const Eigen::Matrix& pointInCamera, - Eigen::Matrix* jacobianWrtPoint) const { +Eigen::Vector CameraProjectionTemplated::project( + const Eigen::Vector& pointInCamera, + Eigen::Ref> jacobianWrtPoint, + Eigen::Ref> jacobianWrtParams) const { return std::visit( [&](auto&& projection) { using T = std::decay_t; - return T::project(pointInCamera, projectionParams_, jacobianWrtPoint); + return T::project( + pointInCamera, + projectionParams_, + isNull(jacobianWrtPoint) ? nullptr : &jacobianWrtPoint, + isNull(jacobianWrtParams) ? nullptr : &jacobianWrtParams); }, projectionVariant_); } template -Eigen::Matrix CameraProjectionTemplated::unproject( - const Eigen::Matrix& cameraPixel) const { +Eigen::Vector CameraProjectionTemplated::unproject( + const Eigen::Vector& cameraPixel) const { return std::visit( [&](auto&& projection) { using T = std::decay_t; @@ -127,7 +167,7 @@ template template [[nodiscard]] CameraProjectionTemplated CameraProjectionTemplated::cast() const { - Eigen::Matrix castedParams = + Eigen::Vector castedParams = projectionParams_.template cast(); auto castedModelName = static_cast::ModelType>(modelName_); diff --git a/core/calibration/camera_projections/CameraProjection.h b/core/calibration/camera_projections/CameraProjection.h index 5d229302c..a303e3772 100644 --- a/core/calibration/camera_projections/CameraProjection.h +++ b/core/calibration/camera_projections/CameraProjection.h @@ -25,6 +25,7 @@ #include #include +#include #include namespace projectaria::tools::calibration { @@ -53,6 +54,26 @@ struct CameraProjectionTemplated { */ CameraProjectionTemplated() = default; + /** + * @brief Default copy constructor. + */ + CameraProjectionTemplated(const CameraProjectionTemplated&) = default; + + /** + * @brief Default move constructor. + */ + CameraProjectionTemplated(CameraProjectionTemplated&&) = default; + + /** + * @brief Default copy assignment. + */ + CameraProjectionTemplated& operator=(const CameraProjectionTemplated&) = default; + + /** + * @brief Default move assignment. + */ + CameraProjectionTemplated& operator=(CameraProjectionTemplated&&) = default; + /** * @brief Constructor with a list of parameters for CameraProjectionTemplated. * @param type The type of projection model, e.g. ModelType::Linear. @@ -60,10 +81,14 @@ struct CameraProjectionTemplated { */ CameraProjectionTemplated( const ModelType& type, - const Eigen::Matrix& projectionParams); + const Eigen::Vector& projectionParams); ModelType modelName() const; - Eigen::Matrix projectionParams() const; + const Eigen::Vector& projectionParams() const; + Eigen::Vector& projectionParamsMut(); + int numParameters() const; // return number of parameters + int numProjectionParameters() const; // return number of projection parameters + int numDistortionParameters() const; // return number of distortion parameters /** * @brief projects a 3d world point in the camera space to a 2d pixel in the image space. No @@ -72,27 +97,30 @@ struct CameraProjectionTemplated { * @param pointInCamera The 3D point in camera space to be projected. * @param jacobianWrtPoint Optional, if not null, will store the Jacobian of the projection * with respect to the point in camera space. + * @param jacobianWrtParams Optional, if not null, will store the Jacobian of the projection + * with respect to the projection parameters. * * @return The 2D pixel coordinates of the projected point in the image space. */ - Eigen::Matrix project( - const Eigen::Matrix& pointInCamera, - Eigen::Matrix* jacobianWrtPoint = nullptr) const; + Eigen::Vector project( + const Eigen::Vector& pointInCamera, + Eigen::Ref> jacobianWrtPoint = NullRef(), + Eigen::Ref> jacobianWrtParams = NullRef()) const; /** * @brief unprojects a 2d pixel in the image space to a 3d world point in homogenous coordinate. * No checks performed in this process. */ - Eigen::Matrix unproject(const Eigen::Matrix& cameraPixel) const; + Eigen::Vector unproject(const Eigen::Vector& cameraPixel) const; /** * @brief returns principal point location as {cx, cy} */ - Eigen::Matrix getPrincipalPoint() const; + Eigen::Vector getPrincipalPoint() const; /** * @brief returns focal lengths as {fx, fy} */ - Eigen::Matrix getFocalLengths() const; + Eigen::Vector getFocalLengths() const; /** * @brief scales the projection parameters as the image scales without the offset changing @@ -120,7 +148,7 @@ struct CameraProjectionTemplated { private: ModelType modelName_; - Eigen::Matrix projectionParams_; + Eigen::Vector projectionParams_; ProjectionVariant projectionVariant_; }; using CameraProjection = CameraProjectionTemplated; diff --git a/core/calibration/camera_projections/FisheyeRadTanThinPrism.h b/core/calibration/camera_projections/FisheyeRadTanThinPrism.h index b6960e01e..ebc401018 100644 --- a/core/calibration/camera_projections/FisheyeRadTanThinPrism.h +++ b/core/calibration/camera_projections/FisheyeRadTanThinPrism.h @@ -71,11 +71,16 @@ class FisheyeRadTanThinPrism { static constexpr bool kIsFisheye = true; static constexpr bool kHasAnalyticalProjection = true; - template > + template < + class D, + class DP, + class DJ1 = Eigen::Matrix, + class DJ2 = Eigen::Matrix> static Eigen::Matrix project( const Eigen::MatrixBase& pointOptical, const Eigen::MatrixBase& params, - Eigen::MatrixBase* d_point = nullptr) { + Eigen::MatrixBase* d_point = nullptr, + Eigen::MatrixBase* d_param = nullptr) { using T = typename D::Scalar; validateProjectInput(); @@ -138,41 +143,90 @@ class FisheyeRadTanThinPrism { } // Maybe compute point jacobian - if (d_point) { + if (d_param || d_point) { Eigen::Matrix duvDistorted_dxryr; compute_duvDistorted_dxryr(xr_yr, xr_yr_squaredNorm, params, duvDistorted_dxryr); // compute jacobian wrt point - Eigen::Matrix duvDistorted_dab; - if (r == static_cast(0.0)) { - duvDistorted_dab.setIdentity(); - } else { - T dthD_dth = static_cast(1.0); - T theta2i = thetaSq; - for (size_t i = 0; i < numK; ++i) { - dthD_dth += T(2 * i + 3) * params[startK + i] * theta2i; - theta2i *= thetaSq; + if (d_point) { + Eigen::Matrix duvDistorted_dab; + if (r == static_cast(0.0)) { + duvDistorted_dab.setIdentity(); + } else { + T dthD_dth = static_cast(1.0); + T theta2i = thetaSq; + for (size_t i = 0; i < numK; ++i) { + dthD_dth += T(2 * i + 3) * params[startK + i] * theta2i; + theta2i *= thetaSq; + } + + const T w1 = dthD_dth / (r_sq + r_sq * r_sq); + const T w2 = th_radial * th_divr / r_sq; + const T ab10 = ab[0] * ab[1]; + Eigen::Matrix temp1; + temp1(0, 0) = w1 * ab_squared[0] + w2 * ab_squared[1]; + temp1(0, 1) = (w1 - w2) * ab10; + temp1(1, 0) = temp1(0, 1); + temp1(1, 1) = w1 * ab_squared[1] + w2 * ab_squared[0]; + duvDistorted_dab.noalias() = duvDistorted_dxryr * temp1; } - const T w1 = dthD_dth / (r_sq + r_sq * r_sq); - const T w2 = th_radial * th_divr / r_sq; - const T ab10 = ab[0] * ab[1]; - Eigen::Matrix temp1; - temp1(0, 0) = w1 * ab_squared[0] + w2 * ab_squared[1]; - temp1(0, 1) = (w1 - w2) * ab10; - temp1(1, 0) = temp1(0, 1); - temp1(1, 1) = w1 * ab_squared[1] + w2 * ab_squared[0]; - duvDistorted_dab.noalias() = duvDistorted_dxryr * temp1; + // compute the derivative of the projection wrt the point: + if (useSingleFocalLength) { + d_point->template leftCols<2>() = params[0] * inv_z * duvDistorted_dab; + } else { + d_point->template leftCols<2>() = + params.template head<2>().asDiagonal() * duvDistorted_dab * inv_z; + } + d_point->template rightCols<1>().noalias() = -d_point->template leftCols<2>() * ab; } - // compute the derivative of the projection wrt the point: - if (useSingleFocalLength) { - d_point->template leftCols<2>() = params[0] * inv_z * duvDistorted_dab; - } else { - d_point->template leftCols<2>() = - params.template head<2>().asDiagonal() * duvDistorted_dab * inv_z; + if (d_param) { + if constexpr (useSingleFocalLength) { + d_param->template leftCols<1>() = uvDistorted; + } else { + d_param->template leftCols<2>() = uvDistorted.asDiagonal(); + } + + d_param->template middleCols<2>(kPrincipalPointColIdx).setIdentity(); + + if (numK > 0) { + Eigen::Matrix temp; + if constexpr (useSingleFocalLength) { + temp = (params[kFocalXIdx] * th_divr) * duvDistorted_dxryr * ab; + } else { + temp = params.template head<2>().cwiseProduct(th_divr * (duvDistorted_dxryr * ab)); + } + T theta2i = thetaSq; + for (size_t i = 0; i < numK; ++i) { + d_param->col(startK + i) = theta2i * temp; + theta2i *= thetaSq; + } + } + + // tangential terms + if (useTangential) { + if constexpr (useSingleFocalLength) { + d_param->template middleCols<2>(startP).noalias() = + T(2) * params[kFocalXIdx] * xr_yr * xr_yr.transpose(); + const T temp3 = params[kFocalXIdx] * xr_yr_squaredNorm; + (*d_param)(0, startP) += temp3; + (*d_param)(1, startP + 1) += temp3; + } else { + d_param->template middleCols<2>(startP).noalias() = + T(2) * xr_yr.cwiseProduct(params.template head<2>()) * xr_yr.transpose(); + (*d_param)(0, startP) += params[kFocalXIdx] * xr_yr_squaredNorm; + (*d_param)(1, startP + 1) += params[kFocalYIdx] * xr_yr_squaredNorm; + } + } + + if (useThinPrism) { + d_param->template block<1, 2>(0, startS) = params[kFocalXIdx] * radialPowers2And4; + d_param->template block<1, 2>(1, startS).setZero(); + d_param->template block<1, 2>(0, startS + 2).setZero(); + d_param->template block<1, 2>(1, startS + 2) = params[kFocalYIdx] * radialPowers2And4; + } } - d_point->template rightCols<1>().noalias() = -d_point->template leftCols<2>() * ab; } // compute the return value diff --git a/core/calibration/camera_projections/KannalaBrandtK3.h b/core/calibration/camera_projections/KannalaBrandtK3.h index fa2849db7..5e90b3500 100644 --- a/core/calibration/camera_projections/KannalaBrandtK3.h +++ b/core/calibration/camera_projections/KannalaBrandtK3.h @@ -50,11 +50,16 @@ class KannalaBrandtK3Projection { static constexpr bool kIsFisheye = true; static constexpr bool kHasAnalyticalProjection = true; - template > + template < + class D, + class DP, + class DJ1 = Eigen::Matrix, + class DJ2 = Eigen::Matrix> static Eigen::Matrix project( const Eigen::MatrixBase& pointOptical, const Eigen::MatrixBase& params, - Eigen::MatrixBase* d_point = nullptr) { + Eigen::MatrixBase* d_point = nullptr, + Eigen::MatrixBase* d_params = nullptr) { validateProjectInput(); using T = typename D::Scalar; @@ -107,14 +112,61 @@ class KannalaBrandtK3Projection { (*d_point) = ff.asDiagonal().toDenseMatrix() * (*d_point); } + if (d_params) { + const T xScaled = pointOptical[0] * params[0] * radiusInverse; + const T yScaled = pointOptical[1] * params[1] * radiusInverse; + + const T theta3 = theta * theta2; + const T theta5 = theta3 * theta2; + const T theta7 = theta5 * theta2; + const T theta9 = theta7 * theta2; + + (*d_params)(0, 0) = pointOptical[0] * scaling; + (*d_params)(0, 1) = T(0.0); + (*d_params)(0, 2) = T(1.0); + (*d_params)(0, 3) = T(0.0); + (*d_params)(0, 4) = xScaled * theta3; + (*d_params)(0, 5) = xScaled * theta5; + (*d_params)(0, 6) = xScaled * theta7; + (*d_params)(0, 7) = xScaled * theta9; + (*d_params)(1, 0) = T(0.0); + (*d_params)(1, 1) = pointOptical[1] * scaling; + (*d_params)(1, 2) = T(0.0); + (*d_params)(1, 3) = T(1.0); + (*d_params)(1, 4) = yScaled * theta3; + (*d_params)(1, 5) = yScaled * theta5; + (*d_params)(1, 6) = yScaled * theta7; + (*d_params)(1, 7) = yScaled * theta9; + } + const Eigen::Matrix px = scaling * ff.cwiseProduct(pointOptical.template head<2>()) + pp; return px; } else { + const T zInv = T(1.0) / pointOptical[2]; + const Eigen::Matrix pointUnitPlane = pointOptical.template head<2>() * zInv; + // linearize r around radius=0 - const Eigen::Matrix px = - ff.cwiseProduct(pointOptical.template head<2>()) / pointOptical(2) + pp; + if (d_point) { + // clang-format off + (*d_point) << ff.x() * zInv, T(0.0), -ff.x() * pointUnitPlane(0) * zInv, + T(0.0), ff.y() * zInv, -ff.y() * pointUnitPlane(1) * zInv; + // clang-format on + } + if (d_params) { + (*d_params)(0, 0) = pointUnitPlane(0); + (*d_params)(0, 1) = T(0.0); + (*d_params)(0, 2) = T(1.0); + (*d_params)(0, 3) = T(0.0); + + (*d_params)(1, 0) = T(0.0); + (*d_params)(1, 1) = pointUnitPlane(1); + (*d_params)(1, 2) = T(0.0); + (*d_params)(1, 3) = T(1.0); + (*d_params).template rightCols<4>().setZero(); + } + const Eigen::Matrix px = ff.cwiseProduct(pointUnitPlane) + pp; return px; } diff --git a/core/calibration/camera_projections/Linear.h b/core/calibration/camera_projections/Linear.h index 04a58d50b..b716dfc0b 100644 --- a/core/calibration/camera_projections/Linear.h +++ b/core/calibration/camera_projections/Linear.h @@ -42,11 +42,16 @@ class LinearProjection { static constexpr bool kIsFisheye = false; static constexpr bool kHasAnalyticalProjection = true; - template > + template < + class D, + class DP, + class DJ1 = Eigen::Matrix, + class DJ2 = Eigen::Matrix> static Eigen::Matrix project( const Eigen::MatrixBase& pointOptical, const Eigen::MatrixBase& params, - Eigen::MatrixBase* d_point = nullptr) { + Eigen::MatrixBase* d_point = nullptr, + Eigen::MatrixBase* d_params = nullptr) { validateProjectInput(); using T = typename D::Scalar; @@ -58,15 +63,25 @@ class LinearProjection { const Eigen::Matrix px = ff.cwiseProduct(pointOptical.template head<2>()) / pointOptical(2) + pp; - if (d_point) { + if (d_point || d_params) { const T oneOverZ = T(1) / pointOptical(2); - (*d_point)(0, 0) = ff(0) * oneOverZ; - (*d_point)(0, 1) = static_cast(0.0); - (*d_point)(0, 2) = -(*d_point)(0, 0) * pointOptical(0) * oneOverZ; - (*d_point)(1, 0) = static_cast(0.0); - (*d_point)(1, 1) = ff(1) * oneOverZ; - (*d_point)(1, 2) = -(*d_point)(1, 1) * pointOptical(1) * oneOverZ; + if (d_point) { + (*d_point)(0, 0) = ff(0) * oneOverZ; + (*d_point)(0, 1) = static_cast(0.0); + (*d_point)(0, 2) = -(*d_point)(0, 0) * pointOptical(0) * oneOverZ; + (*d_point)(1, 0) = static_cast(0.0); + (*d_point)(1, 1) = ff(1) * oneOverZ; + (*d_point)(1, 2) = -(*d_point)(1, 1) * pointOptical(1) * oneOverZ; + } + + if (d_params) { + (*d_params)(0, 0) = pointOptical(0) * oneOverZ; + (*d_params)(0, 1) = static_cast(0.0); + (*d_params)(1, 0) = static_cast(0.0); + (*d_params)(1, 1) = pointOptical(0) * oneOverZ; + d_params->template rightCols<2>().setIdentity(); + } } return px; diff --git a/core/calibration/camera_projections/Spherical.h b/core/calibration/camera_projections/Spherical.h index b06c3f864..73f38df43 100644 --- a/core/calibration/camera_projections/Spherical.h +++ b/core/calibration/camera_projections/Spherical.h @@ -49,12 +49,17 @@ class SphericalProjection { // // Return 2-point in the image plane. // - template > + template < + class D, + class DP, + class DJ1 = Eigen::Matrix, + class DJ2 = Eigen::Matrix> static Eigen::Matrix project( const Eigen::MatrixBase& pointOptical, const Eigen::MatrixBase& params, - Eigen::MatrixBase* d_point = nullptr) { - if (d_point != nullptr) { + Eigen::MatrixBase* d_point = nullptr, + Eigen::MatrixBase* d_params = nullptr) { + if (d_point != nullptr || d_params != nullptr) { throw std::runtime_error("Jacobians not implemented in Spherical projection model"); } diff --git a/core/calibration/camera_projections/test/CameraProjectionTest.cpp b/core/calibration/camera_projections/test/CameraProjectionTest.cpp index 4880deb83..08b9a7ed3 100644 --- a/core/calibration/camera_projections/test/CameraProjectionTest.cpp +++ b/core/calibration/camera_projections/test/CameraProjectionTest.cpp @@ -136,7 +136,6 @@ TEST(CameraProjectionTest, FloatDoubleParamsComparison) { TEST(CameraProjectionTest, Jacobian) { // Set up problem - Eigen::Matrix dProj_dPointCam; const double max_norm = 1e-4; for (const auto& [modelType, projectionParams] : kTestProjectionParams) { @@ -148,8 +147,10 @@ TEST(CameraProjectionTest, Jacobian) { CameraProjectionTemplated cameraProjectionDouble(modelType, projectionParams); // Project at X and compute Jacobian + Eigen::Matrix dProj_dPointCam; + Eigen::MatrixXd dProj_dCalib(2, cameraProjectionDouble.numParameters()); Eigen::Matrix fX0 = - cameraProjectionDouble.project(kPtInCameraDouble, &dProj_dPointCam); + cameraProjectionDouble.project(kPtInCameraDouble, dProj_dPointCam, dProj_dCalib); // Check Jacobian with finite difference approximation double eps = 1e-8; @@ -171,5 +172,24 @@ TEST(CameraProjectionTest, Jacobian) { // We expect the approximate derivative to be close to the actual derivative double norm = (J - dProj_dPointCam).norm(); EXPECT_NEAR(norm, 0.0, max_norm); + + // do the same for calibration jacobian + Eigen::MatrixXd J_calib(2, cameraProjectionDouble.numParameters()); + for (int i = 0; i < J_calib.cols(); ++i) { + CameraProjectionTemplated cameraProjectionDoubleP = cameraProjectionDouble; + CameraProjectionTemplated cameraProjectionDoubleN = cameraProjectionDouble; + + cameraProjectionDoubleP.projectionParamsMut()[i] += eps; + cameraProjectionDoubleN.projectionParamsMut()[i] -= eps; + + const Eigen::Matrix fxp = cameraProjectionDoubleP.project(kPtInCameraDouble); + const Eigen::Matrix fxn = cameraProjectionDoubleN.project(kPtInCameraDouble); + + J_calib.col(i) = (fxp - fxn) / (2 * eps); + } + + // We expect the approximate derivative to be close to the actual derivative + double normCalib = (J_calib - dProj_dCalib).norm(); + EXPECT_NEAR(normCalib, 0.0, max_norm); } } diff --git a/core/calibration/utility/CMakeLists.txt b/core/calibration/utility/CMakeLists.txt index fc34ec463..e97cb84f6 100644 --- a/core/calibration/utility/CMakeLists.txt +++ b/core/calibration/utility/CMakeLists.txt @@ -15,3 +15,8 @@ add_library(calibration_distort Distort.cpp Distort.h) target_include_directories(calibration_distort PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../..) target_link_libraries(calibration_distort PUBLIC device_calibration image_distort) + +add_library(calibration_nullref INTERFACE) +target_sources(calibration_nullref INTERFACE NullRef.h) +target_link_libraries(calibration_nullref INTERFACE Eigen3::Eigen) +target_include_directories(calibration_nullref INTERFACE $) diff --git a/core/calibration/utility/NullRef.h b/core/calibration/utility/NullRef.h new file mode 100644 index 000000000..14c4b63e5 --- /dev/null +++ b/core/calibration/utility/NullRef.h @@ -0,0 +1,78 @@ +/* + * Copyright (c) Meta Platforms, Inc. and affiliates. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once + +#include + +namespace projectaria::tools::calibration { + +/// Get an Eigen reference pointing to a nullptr. +template < + typename Scalar, + int Rows = Eigen::Dynamic, + int Columns = Eigen::Dynamic, + int StorageOrder = Eigen::ColMajor> +constexpr Eigen::Ref> nullRef( + const int rows = Rows != Eigen::Dynamic ? Rows : 0, + const int columns = Columns != Eigen::Dynamic ? Columns : 0) { + return Eigen::Map>(nullptr, rows, columns); +} + +/// Test if an Eigen Reference is pointing to a nullptr. +template +constexpr bool isNull(const Eigen::Ref& ref) { + return ref.data() == nullptr; +} + +/** + * A helper struct that allows direct assignments of NullRefs as function default + * arguments. + */ +struct NullRef { + /// The default constructor for general usecases. + NullRef() = default; + + /// There might be edge cases where a dynamic sized NullRef is required. This + /// provides the option, while it should generally not be used. + NullRef(const int numRows, const int numCols) : numRows_(numRows), numCols_(numCols) {} + + /// Fixed size matrices: + template + operator Eigen::Ref>() { + return nullRef(); + } + template + operator Eigen::Ref>() { + return nullRef(); + } + + /// Dynamic Matrices: + template + operator Eigen::Ref>() { + return nullRef(numRows_, numCols_); + } + template + operator Eigen::Ref>() { + return nullRef(numRows_, numCols_); + } + + private: + const int numRows_ = 0; + const int numCols_ = 0; +}; + +} // namespace projectaria::tools::calibration diff --git a/core/python/DeviceCalibrationPyBind.h b/core/python/DeviceCalibrationPyBind.h index 44de0902a..4204bdbbc 100644 --- a/core/python/DeviceCalibrationPyBind.h +++ b/core/python/DeviceCalibrationPyBind.h @@ -86,11 +86,23 @@ inline void declareCameraCalibration(py::module& m) { .def("projection_params", &CameraProjection::projectionParams) .def( "project", - &CameraProjection::project, + [](const CameraProjection& proj, + const Eigen::Vector3d& point, + std::optional>> jPoint, + std::optional>> jCalib) + -> std::optional { + return proj.project(point, jPoint ? *jPoint : NullRef(), jCalib ? *jCalib : NullRef()); + }, py::arg("point_in_camera"), - py::arg("jacobian_wrt_point") = nullptr, + py::arg("jacobian_wrt_point") = py::none(), + py::arg("jacobian_wrt_params") = py::none(), "projects a 3d world point in the camera space to a 2d pixel in the image space." - " No checks performed in this process.") + " No checks performed in this process. Jacobian arguments, if specified, must be" + " in Fortran style (ie. column-major order):\n" + "\n" + "jacPoint = numpy.empty((3,4), dtype=np.float64, order='F')\n" + "jacParams = numpy.empty((2, camera.num_parameters()), dtype=np.float64, order='F'))\n" + "pixel = camera.project_no_checks(world_pt, jacPoint, jacParams)") .def( "unproject", &CameraProjection::unproject, @@ -278,16 +290,47 @@ inline void declareCameraCalibration(py::module& m) { .def("get_projection_params", &CameraCalibration::projectionParams) .def( "project_no_checks", - &CameraCalibration::projectNoChecks, + [](const CameraCalibration& calib, + const Eigen::Vector3d& point, + std::optional>> jPoint, + std::optional>> jCalib) + -> std::optional { + return calib.projectNoChecks( + point, jPoint ? *jPoint : NullRef(), jCalib ? *jCalib : NullRef()); + }, py::arg("point_in_camera"), + py::arg("jacobian_wrt_point") = py::none(), + py::arg("jacobian_wrt_params") = py::none(), "Function to project a 3d point (in camera frame) to a 2d camera pixel location. In this" - " function, no check is performed.") + " function, no check is performed. Jacobian arguments, if specified, must be" + " in Fortran style (ie. column-major order):\n" + "\n" + "jacPoint = numpy.empty((3,4), dtype=np.float64, order='F')\n" + "jacParams = numpy.empty((2, camera.num_parameters()), dtype=np.float64, order='F'))\n" + "pixel = camera.project_no_checks(world_pt, jacPoint, jacParams)") .def( "project", - &CameraCalibration::project, + [](const CameraCalibration& calib, + const Eigen::Vector3d& point, + std::optional>> jPoint, + std::optional>> jCalib) + -> std::optional { + return calib.project(point, jPoint ? *jPoint : NullRef(), jCalib ? *jCalib : NullRef()); + }, py::arg("point_in_camera"), + py::arg("jacobian_wrt_point") = py::none(), + py::arg("jacobian_wrt_params") = py::none(), "Function to project a 3d point (in camera frame) to a 2d camera pixel location, with a" - " number of validity checks to ensure the point is visible.") + " number of validity checks to ensure the point is visible. Jacobian arguments, if specified," + " must be in Fortran style (ie. column-major order):\n" + "\n" + "jacPoint = numpy.empty((3,4), dtype=np.float64, order='F')\n" + "jacParams = numpy.empty((2, camera.num_parameters()), dtype=np.float64, order='F'))\n" + "pixel = camera.project(world_pt, jacPoint, jacParams)") + .def( + "num_parameters", + &CameraCalibration::numParameters, + "Return number of calibration parameters.") .def( "unproject_no_checks", &CameraCalibration::unprojectNoChecks,