Calib jacobians

core/calibration/CameraCalibration.cpp

@@ -30,7 +30,7 @@ CameraCalibration::CameraCalibration(
     const double maxSolidAngle,
     const std::string& serialNumber,
     const double timeOffsetSecDeviceCamera,
-    const std::optional<double> maybeReadOutTimeSec)
+    const std::optional<double>& 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<double> CameraCalibration::getReadOutTimeSec() const {
   return maybeReadOutTimeSec_;
 }
 
+std::optional<double>& 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<double, 2, 3>* jacobianWrtPoint,
+    Eigen::Matrix<double, 2, Eigen::Dynamic>* jacobianWrtParams) const {
+  return projectionModel_.project(pointInCamera, jacobianWrtPoint, jacobianWrtParams);
 }
 
 std::optional<Eigen::Vector2d> CameraCalibration::project(
-    const Eigen::Vector3d& pointInCamera) const {
+    const Eigen::Vector3d& pointInCamera,
+    Eigen::Matrix<double, 2, 3>* jacobianWrtPoint,
+    Eigen::Matrix<double, 2, Eigen::Dynamic>* 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;
     }

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<double> maybeReadOutTimeSec = {});
+      const std::optional<double>& maybeReadOutTimeSec = {});
 
   std::string getLabel() const;
   std::string getSerialNumber() const;
@@ -76,7 +81,9 @@ class CameraCalibration {
   double getMaxSolidAngle() const;
   std::optional<double> getValidRadius() const;
   double getTimeOffsetSecDeviceCamera() const;
+  double& getTimeOffsetSecDeviceCameraMut();
   std::optional<double> getReadOutTimeSec() const;
+  std::optional<double>& 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<double, 2, 3>* jacobianWrtPoint = nullptr,
+      Eigen::Matrix<double, 2, Eigen::Dynamic>* 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<Eigen::Vector2d> project(const Eigen::Vector3d& pointInCamera) const;
+  std::optional<Eigen::Vector2d> project(const Eigen::Vector3d& pointInCamera,
+      Eigen::Matrix<double, 2, 3>* jacobianWrtPoint = nullptr,
+      Eigen::Matrix<double, 2, Eigen::Dynamic>* 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.

core/calibration/camera_projections/CameraProjection.cpp

@@ -39,7 +39,7 @@ typename CameraProjectionTemplated<Scalar>::ProjectionVariant getProjectionVaria
 template <typename Scalar>
 CameraProjectionTemplated<Scalar>::CameraProjectionTemplated(
     const ModelType& type,
-    const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& projectionParams)
+    const Eigen::Vector<Scalar, Eigen::Dynamic>& projectionParams)
     : modelName_(type),
       projectionParams_(projectionParams),
       projectionVariant_(getProjectionVariant<Scalar>(type)) {}
@@ -51,15 +51,50 @@ typename CameraProjectionTemplated<Scalar>::ModelType CameraProjectionTemplated<
 }
 
 template <typename Scalar>
-Eigen::Matrix<Scalar, Eigen::Dynamic, 1> CameraProjectionTemplated<Scalar>::projectionParams()
+const Eigen::Vector<Scalar, Eigen::Dynamic>& CameraProjectionTemplated<Scalar>::projectionParams() 
     const {
   return projectionParams_;
 }
 
 template <typename Scalar>
-Eigen::Matrix<Scalar, 2, 1> CameraProjectionTemplated<Scalar>::getFocalLengths() const {
+Eigen::Vector<Scalar, Eigen::Dynamic>& CameraProjectionTemplated<Scalar>::projectionParamsMut() {
+  return projectionParams_;
+}
+
+template <typename Scalar>
+int CameraProjectionTemplated<Scalar>::numParameters() const {
+  return std::visit(
+      [this](auto&& projection) -> int {
+        using T = std::decay_t<decltype(projection)>;
+        return T::kNumParams;
+      },
+      projectionVariant_);
+}
+
+template <typename Scalar>
+int CameraProjectionTemplated<Scalar>::numProjectionParameters() const {
+  return std::visit(
+      [this](auto&& projection) -> int {
+        using T = std::decay_t<decltype(projection)>;
+        return T::kNumParams - T::kNumDistortionParams;
+      },
+      projectionVariant_);
+}
+
+template <typename Scalar>
+int CameraProjectionTemplated<Scalar>::numDistortionParameters() const {
+  return std::visit(
+      [this](auto&& projection) -> int {
+        using T = std::decay_t<decltype(projection)>;
+        return T::kNumDistortionParams;
+      },
+      projectionVariant_);
+}
+
+template <typename Scalar>
+Eigen::Vector<Scalar, 2> CameraProjectionTemplated<Scalar>::getFocalLengths() const {
   return std::visit(
-      [this](auto&& projection) -> Eigen::Matrix<Scalar, 2, 1> {
+      [this](auto&& projection) -> Eigen::Vector<Scalar, 2> {
         using T = std::decay_t<decltype(projection)>;
         int focalXIdx = T::kFocalXIdx;
         int focalYIdx = T::kFocalYIdx;
@@ -69,9 +104,9 @@ Eigen::Matrix<Scalar, 2, 1> CameraProjectionTemplated<Scalar>::getFocalLengths()
 }
 
 template <typename Scalar>
-Eigen::Matrix<Scalar, 2, 1> CameraProjectionTemplated<Scalar>::getPrincipalPoint() const {
+Eigen::Vector<Scalar, 2> CameraProjectionTemplated<Scalar>::getPrincipalPoint() const {
   return std::visit(
-      [this](auto&& projection) -> Eigen::Matrix<Scalar, 2, 1> {
+      [this](auto&& projection) -> Eigen::Vector<Scalar, 2> {
         using T = std::decay_t<decltype(projection)>;
         int principalPointColIdx = T::kPrincipalPointColIdx;
         int principalPointRowIdx = T::kPrincipalPointRowIdx;
@@ -81,20 +116,21 @@ Eigen::Matrix<Scalar, 2, 1> CameraProjectionTemplated<Scalar>::getPrincipalPoint
 }
 
 template <typename Scalar>
-Eigen::Matrix<Scalar, 2, 1> CameraProjectionTemplated<Scalar>::project(
-    const Eigen::Matrix<Scalar, 3, 1>& pointInCamera,
-    Eigen::Matrix<Scalar, 2, 3>* jacobianWrtPoint) const {
+Eigen::Vector<Scalar, 2> CameraProjectionTemplated<Scalar>::project(
+    const Eigen::Vector<Scalar, 3>& pointInCamera,
+    Eigen::Matrix<Scalar, 2, 3>* jacobianWrtPoint,
+    Eigen::Matrix<Scalar, 2, Eigen::Dynamic>* jacobianWrtParams) const {
   return std::visit(
       [&](auto&& projection) {
         using T = std::decay_t<decltype(projection)>;
-        return T::project(pointInCamera, projectionParams_, jacobianWrtPoint);
+        return T::project(pointInCamera, projectionParams_, jacobianWrtPoint, jacobianWrtParams);
       },
       projectionVariant_);
 }
 
 template <typename Scalar>
-Eigen::Matrix<Scalar, 3, 1> CameraProjectionTemplated<Scalar>::unproject(
-    const Eigen::Matrix<Scalar, 2, 1>& cameraPixel) const {
+Eigen::Vector<Scalar, 3> CameraProjectionTemplated<Scalar>::unproject(
+    const Eigen::Vector<Scalar, 2>& cameraPixel) const {
   return std::visit(
       [&](auto&& projection) {
         using T = std::decay_t<decltype(projection)>;
@@ -127,7 +163,7 @@ template <typename Scalar>
 template <typename OtherScalar>
 [[nodiscard]] CameraProjectionTemplated<OtherScalar> CameraProjectionTemplated<Scalar>::cast()
     const {
-  Eigen::Matrix<OtherScalar, Eigen::Dynamic, 1> castedParams =
+  Eigen::Vector<OtherScalar, Eigen::Dynamic> castedParams =
       projectionParams_.template cast<OtherScalar>();
   auto castedModelName =
       static_cast<typename CameraProjectionTemplated<OtherScalar>::ModelType>(modelName_);

core/calibration/camera_projections/CameraProjection.h

@@ -53,17 +53,26 @@ 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.
    * @param projectionParams The projection parameters.
    */
   CameraProjectionTemplated(
       const ModelType& type,
-      const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& projectionParams);
+      const Eigen::Vector<Scalar, Eigen::Dynamic>& projectionParams);
 
   ModelType modelName() const;
-  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> projectionParams() const;
+  const Eigen::Vector<Scalar, Eigen::Dynamic>& projectionParams() const;
+  Eigen::Vector<Scalar, Eigen::Dynamic>& 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<Scalar, 2, 1> project(
-      const Eigen::Matrix<Scalar, 3, 1>& pointInCamera,
-      Eigen::Matrix<Scalar, 2, 3>* jacobianWrtPoint = nullptr) const;
+  Eigen::Vector<Scalar, 2> project(
+      const Eigen::Vector<Scalar, 3>& pointInCamera,
+      Eigen::Matrix<Scalar, 2, 3>* jacobianWrtPoint = nullptr,
+      Eigen::Matrix<Scalar, 2, Eigen::Dynamic>* 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<Scalar, 3, 1> unproject(const Eigen::Matrix<Scalar, 2, 1>& cameraPixel) const;
+  Eigen::Vector<Scalar, 3> unproject(const Eigen::Vector<Scalar, 2>& cameraPixel) const;
 
   /**
    * @brief returns principal point location as {cx, cy}
    */
-  Eigen::Matrix<Scalar, 2, 1> getPrincipalPoint() const;
+  Eigen::Vector<Scalar, 2> getPrincipalPoint() const;
   /**
    * @brief returns focal lengths as {fx, fy}
    */
-  Eigen::Matrix<Scalar, 2, 1> getFocalLengths() const;
+  Eigen::Vector<Scalar, 2> 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<Scalar, Eigen::Dynamic, 1> projectionParams_;
+  Eigen::Vector<Scalar, Eigen::Dynamic> projectionParams_;
   ProjectionVariant projectionVariant_;
 };
 using CameraProjection = CameraProjectionTemplated<double>;