diff --git a/core/calibration/CameraCalibration.cpp b/core/calibration/CameraCalibration.cpp index a0f03d9e5..f946cbe55 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,19 @@ 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::Matrix* jacobianWrtPoint, + Eigen::Matrix* jacobianWrtParams) const { + return projectionModel_.project(pointInCamera, jacobianWrtPoint, jacobianWrtParams); } std::optional CameraCalibration::project( - const Eigen::Vector3d& pointInCamera) const { + const Eigen::Vector3d& pointInCamera, + Eigen::Matrix* jacobianWrtPoint, + Eigen::Matrix* 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..9934797c1 100644 --- a/core/calibration/CameraCalibration.h +++ b/core/calibration/CameraCalibration.h @@ -37,6 +37,11 @@ class CameraCalibration { */ CameraCalibration() = default; + /** + * @brief Default copy constructor. + */ + CameraCalibration(const 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 +72,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 +81,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 +92,39 @@ 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::Matrix* jacobianWrtPoint = nullptr, + Eigen::Matrix* jacobianWrtParams = nullptr) 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::Matrix* jacobianWrtPoint = nullptr, + Eigen::Matrix* jacobianWrtParams = nullptr) const; /** * @brief Function to unproject a 2d pixel location to a 3d ray in camera frame. In this function, @@ -109,6 +133,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/CameraProjection.cpp b/core/calibration/camera_projections/CameraProjection.cpp index 42ac6e0dd..c3c416b1b 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( + [this](auto&& projection) -> int { + using T = std::decay_t; + return T::kNumParams; + }, + projectionVariant_); +} + +template +int CameraProjectionTemplated::numProjectionParameters() const { + return std::visit( + [this](auto&& projection) -> int { + using T = std::decay_t; + return T::kNumParams - T::kNumDistortionParams; + }, + projectionVariant_); +} + +template +int CameraProjectionTemplated::numDistortionParameters() const { + return std::visit( + [this](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,21 @@ 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::Matrix* jacobianWrtPoint, + Eigen::Matrix* jacobianWrtParams) const { return std::visit( [&](auto&& projection) { using T = std::decay_t; - return T::project(pointInCamera, projectionParams_, jacobianWrtPoint); + return T::project(pointInCamera, projectionParams_, jacobianWrtPoint, 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 +163,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..938373688 100644 --- a/core/calibration/camera_projections/CameraProjection.h +++ b/core/calibration/camera_projections/CameraProjection.h @@ -53,6 +53,11 @@ struct CameraProjectionTemplated { */ CameraProjectionTemplated() = default; + /** + * @brief Default copy constructor. + */ + CameraProjectionTemplated(const CameraProjectionTemplated&) = default; + /** * @brief Constructor with a list of parameters for CameraProjectionTemplated. * @param type The type of projection model, e.g. ModelType::Linear. @@ -60,10 +65,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 +81,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::Matrix* jacobianWrtPoint = nullptr, + Eigen::Matrix* jacobianWrtParams = nullptr) 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 +132,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..7fc4dab82 100644 --- a/core/calibration/camera_projections/FisheyeRadTanThinPrism.h +++ b/core/calibration/camera_projections/FisheyeRadTanThinPrism.h @@ -71,11 +71,14 @@ class FisheyeRadTanThinPrism { static constexpr bool kIsFisheye = true; static constexpr bool kHasAnalyticalProjection = true; - template > + template , + 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 +141,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..251320320 100644 --- a/core/calibration/camera_projections/KannalaBrandtK3.h +++ b/core/calibration/camera_projections/KannalaBrandtK3.h @@ -50,11 +50,15 @@ class KannalaBrandtK3Projection { static constexpr bool kIsFisheye = true; static constexpr bool kHasAnalyticalProjection = true; - template > + template , + 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 +111,62 @@ class KannalaBrandtK3Projection { (*d_point) = ff.asDiagonal().toDenseMatrix() * (*d_point); } + if (d_params) { + const Eigen::Matrix pointUnitPlane = pointOptical.template head<2>() / pointOptical.z(); + const T xScaled = pointUnitPlane[0] * params[0] * radiusInverse; + const T yScaled = pointUnitPlane[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) = pointUnitPlane[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) = pointUnitPlane[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 { + } 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..e859901af 100644 --- a/core/calibration/camera_projections/Linear.h +++ b/core/calibration/camera_projections/Linear.h @@ -42,11 +42,15 @@ class LinearProjection { static constexpr bool kIsFisheye = false; static constexpr bool kHasAnalyticalProjection = true; - template > + template , + 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 +62,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..feec2a1ba 100644 --- a/core/calibration/camera_projections/Spherical.h +++ b/core/calibration/camera_projections/Spherical.h @@ -49,12 +49,16 @@ class SphericalProjection { // // Return 2-point in the image plane. // - template > + template , + 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/python/DeviceCalibrationPyBind.h b/core/python/DeviceCalibrationPyBind.h index 1a50c4fd3..9d8a59c48 100644 --- a/core/python/DeviceCalibrationPyBind.h +++ b/core/python/DeviceCalibrationPyBind.h @@ -88,6 +88,7 @@ inline void declareCameraCalibration(py::module& m) { &CameraProjection::project, py::arg("point_in_camera"), py::arg("jacobian_wrt_point") = nullptr, + py::arg("jacobian_wrt_params") = nullptr, "projects a 3d world point in the camera space to a 2d pixel in the image space." " No checks performed in this process.") .def( @@ -277,12 +278,16 @@ inline void declareCameraCalibration(py::module& m) { "project_no_checks", &CameraCalibration::projectNoChecks, py::arg("point_in_camera"), + py::arg("jacobian_wrt_point") = nullptr, + py::arg("jacobian_wrt_params") = nullptr, "Function to project a 3d point (in camera frame) to a 2d camera pixel location. In this" " function, no check is performed.") .def( "project", &CameraCalibration::project, py::arg("point_in_camera"), + py::arg("jacobian_wrt_point") = nullptr, + py::arg("jacobian_wrt_params") = nullptr, "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.") .def(