Initial DH Chain Kinematic Calibration functionality

rct_optimizations/CMakeLists.txt

@@ -28,9 +28,11 @@ add_library(${PROJECT_NAME} SHARED
   src/${PROJECT_NAME}/validation/noise_qualification.cpp
   #Target Homography
   src/${PROJECT_NAME}/validation/homography_validation.cpp
+  # DH Chain Kinematic Calibration
+  src/${PROJECT_NAME}/dh_chain.cpp
+  src/${PROJECT_NAME}/dh_chain_kinematic_calibration.cpp
 )
-
-target_compile_options(${PROJECT_NAME} PUBLIC -std=c++14)
+target_compile_options(${PROJECT_NAME} PUBLIC -std=c++11)
 target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra)
 target_include_directories(${PROJECT_NAME} PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"

rct_optimizations/include/rct_optimizations/dh_chain.h

@@ -7,31 +7,86 @@
 
 namespace rct_optimizations
 {
+/**
+ * @brief The joint types for DH representation
+ */
 enum class DHJointType : unsigned
 {
   LINEAR,
   REVOLUTE,
   FIXED
 };
 
+template<typename T>
+using Isometry3 = Eigen::Transform<T, 3, Eigen::Isometry>;
+
+template<typename T>
+using Vector4 = Eigen::Matrix<T, 4, 1>;
+
+template<typename T>
+using Vector3 = Eigen::Matrix<T, 3, 1>;
+
+template<typename T>
+using Vector2 = Eigen::Matrix<T, 2, 1>;
+
 /**
  * @brief Struct representing the DH parameters of a single transformation between adjacent links.
  * This struct follows the classical DH parameter convention: Trans[Zi-1](d) * Rot[Zi-1](theta) * Trans[Xi](r) * Rot[Xi](alpha)
  * See @link https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters for reference
  */
 struct DHTransform
 {
-  using Ptr = std::unique_ptr<DHTransform>;
-
-  DHTransform(std::array<double, 4> params_, DHJointType type_);
-  virtual ~DHTransform() = default;
+  DHTransform(const Eigen::Vector4d& params_, DHJointType type_);
 
   /**
    * @brief Creates the homogoneous transformation from the previous link to the current link
-   * @param joint_value
+   * @param joint_value - The joint value to apply when caluclating the transform
+   * @param offsets - The DH parameter offsets to apply when calculating the transform
    * @return
    */
-  virtual Eigen::Isometry3d createRelativeTransform(const double joint_value) const;
+  template<typename T>
+  Isometry3<T> createRelativeTransform(const T joint_value,
+                                       const Eigen::Matrix<T, 1, 4>& offsets) const
+  {
+    Isometry3<T> transform(Isometry3<T>::Identity());
+
+    // Create an DH parameter vector with offsets ([d, theta, r, alpha]
+    Vector4<T> updated_params = params.cast<T>() + offsets.transpose();
+
+    switch (type)
+    {
+    case DHJointType::LINEAR:
+      // Add the joint value to d (index 0) if the joint is linear
+      updated_params(0) += joint_value;
+      break;
+    case DHJointType::REVOLUTE:
+      // Add the joint value to theta (index 1) if the joint is revolute
+      updated_params(1) += joint_value;
+      break;
+    default:
+      break;
+    }
+
+    // Perform the DH transformations
+    transform *= Eigen::Translation<T, 3>(T(0.0), T(0.0), updated_params(0));
+    transform.rotate(Eigen::AngleAxis<T>(updated_params(1), Vector3<T>::UnitZ()));
+    transform *= Eigen::Translation<T, 3>(updated_params(2), T(0.0), T(0.0));
+    transform.rotate(Eigen::AngleAxis<T>(updated_params(3), Vector3<T>::UnitX()));
+
+    return transform;
+  }
+
+  /**
+   * @brief Creates the homogoneous transformation from the previous link to the current link without applying DH parameter offsets
+   * @param joint_value - The joint value to apply when calculating the transform
+   * @return
+   */
+  template<typename T>
+  Isometry3<T> createRelativeTransform(const T joint_value) const
+  {
+    Eigen::Matrix<T, 1, 4> offsets = Eigen::Matrix<T, 1, 4>::Zero();
+    return createRelativeTransform(joint_value, offsets);
+  }
 
   double createRandomJointValue() const;
 
@@ -41,35 +96,19 @@ struct DHTransform
    *  r: The linear offset in X
    *  alpha: The rotational offset about X
    */
-  std::array<double, 4> params;
+  Eigen::Vector4d params;
   DHJointType type; /** @brief The type of actuation of the joint */
   double max = M_PI; /** @brief Joint max */
   double min = -M_PI; /** @brief Joint min */
 };
 
-/**
- * @brief Override of the @ref DHTransform class to provide Gaussian noise for the joint value when calculating forward kinematics
- */
-struct GaussianNoiseDHTransform : public DHTransform
-{
-  GaussianNoiseDHTransform(std::array<double, 4> params_,
-                           DHJointType type_,
-                           double mean_,
-                           double std_dev_);
-
-  virtual Eigen::Isometry3d createRelativeTransform(const double joint) const override;
-
-  double mean;
-  double std_dev;
-};
-
 /**
  * @brief Robot representation using DH parameters
  */
 class DHChain
 {
 public:
-    DHChain(std::vector<DHTransform::Ptr> transforms);
+    DHChain(std::vector<DHTransform> transforms);
 
   /**
    * @brief Calculates forward kinematics for the robot with the joints provided.
@@ -78,20 +117,63 @@ class DHChain
    * @return
    * @throws Exception if the size of joint values is larger than the number of DH transforms in the robot
    */
-  Eigen::Isometry3d getFK(const std::vector<double> &joint_values) const;
+  template<typename T>
+  Isometry3<T> getFK(const Eigen::Matrix<T, Eigen::Dynamic, 1> &joint_values) const
+  {
+    Isometry3<T> transform(Isometry3<T>::Identity());
+    for (Eigen::Index i = 0; i < joint_values.size(); ++i)
+    {
+      transform = transform * transforms_.at(i).createRelativeTransform(joint_values[i]);
+    }
+    return transform;
+  }
 
   /**
    * @brief Override function of @ref getFK but using a data pointer for easier integration with Ceres
    * @param joint_values
-   * @param n_joints
+   * @param offsets
+   * @return
+   */
+  template<typename T>
+  Isometry3<T> getFK(const Eigen::Matrix<T, Eigen::Dynamic, 1>& joint_values,
+                     const Eigen::Matrix<T, Eigen::Dynamic, 4>& offsets) const
+  {
+    Isometry3<T> transform(Isometry3<T>::Identity());
+    for (Eigen::Index i = 0; i < joint_values.size(); ++i)
+    {
+      const Eigen::Matrix<T, 1, 4> &offset = offsets.row(i);
+      transform = transform * transforms_.at(i).createRelativeTransform(joint_values[i], offset);
+    }
+    return transform;
+  }
+
+  /**
+   * @brief Creates a random joint pose by choosing a random uniformly distributed joint value for each joint in the chain
+   * @return
+   */
+  Eigen::VectorXd createUniformlyRandomPose() const;
+
+  /**
+   * @brief Returns the number of degrees of freedom (i.e. DH transforms) of the chain
+   * @return
+   */
+  std::size_t dof() const;
+
+  /**
+   * @brief Gets a dof x 4 matrix of the DH parameters
    * @return
    */
-  Eigen::Isometry3d getFK(const double *joint_values, const std::size_t n_joints) const;
+  Eigen::MatrixX4d getDHTable() const;
 
-  Eigen::Isometry3d createUniformlyRandomPose() const;
+  /**
+   * @brief Returns a the joint types of the kinematic chain in order
+   * @return
+   */
+  std::vector<DHJointType> getJointTypes() const;
 
 protected:
-  std::vector<DHTransform::Ptr> transforms_;
+  /** @brief The DH transforms that make up the chain */
+  std::vector<DHTransform> transforms_;
 };
 
 } // namespace rct_optimizations

rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h

@@ -0,0 +1,165 @@
+#pragma once
+
+#include <rct_optimizations/types.h>
+#include <rct_optimizations/dh_chain.h>
+#include <rct_optimizations/ceres_math_utilities.h>
+
+namespace rct_optimizations
+{
+struct KinematicCalibrationProblem2D3D
+{
+  KinematicCalibrationProblem2D3D(DHChain camera_chain_, DHChain target_chain_)
+    : camera_chain(std::move(camera_chain_))
+    , target_chain(std::move(target_chain_))
+    , camera_mount_to_camera_guess(Eigen::Isometry3d::Identity())
+    , target_mount_to_target_guess(Eigen::Isometry3d::Identity())
+    , camera_base_to_target_base_guess(Eigen::Isometry3d::Identity())
+  {
+  }
+
+  DHChain camera_chain;
+  DHChain target_chain;
+  KinObservation2D3D::Set observations;
+  CameraIntrinsics intr;
+  Eigen::Isometry3d camera_mount_to_camera_guess;
+  Eigen::Isometry3d target_mount_to_target_guess;
+  Eigen::Isometry3d camera_base_to_target_base_guess;
+};
+
+struct KinematicCalibrationResult
+{
+  bool converged;
+  double initial_cost_per_obs;
+  double final_cost_per_obs;
+  Eigen::Isometry3d camera_mount_to_camera;
+  Eigen::Isometry3d target_mount_to_target;
+  Eigen::Isometry3d camera_base_to_target_base;
+  Eigen::MatrixX4d camera_chain_dh_offsets;
+  Eigen::MatrixX4d target_chain_dh_offsets;
+
+  // TODO: Add covariance matrix/matrices
+//  Eigen::MatrixXd covariance;
+};
+
+class DualDHChainCost2D3D
+{
+  public:
+  DualDHChainCost2D3D(const Eigen::Vector2d &obs,
+       const Eigen::Vector3d &point_in_target,
+       const CameraIntrinsics &intr,
+       const DHChain &camera_chain,
+       const DHChain &target_chain,
+       const Eigen::VectorXd &camera_chain_joints,
+       const Eigen::VectorXd &target_chain_joints)
+    : obs_(obs)
+    , target_pt_(point_in_target)
+    , intr_(intr)
+    , camera_chain_(camera_chain)
+    , target_chain_(target_chain)
+    , camera_chain_joints_(camera_chain_joints)
+    , target_chain_joints_(target_chain_joints)
+  {
+  }
+
+  template<typename T>
+  Isometry3<T> createTransform(T const *const *params, const std::size_t idx) const
+  {
+    Eigen::Map<const Vector3<T>> t(params[idx]);
+    Eigen::Map<const Vector3<T>> aa(params[idx + 1]);
+
+    Isometry3<T> result = Isometry3<T>::Identity() * Eigen::Translation<T, 3>(t);
+
+    T aa_norm = aa.norm();
+    if (aa_norm > std::numeric_limits<T>::epsilon())
+    {
+      result *= Eigen::AngleAxis<T>(aa_norm, aa.normalized());
+    }
+
+    return result;
+  }
+
+  static std::vector<double *> constructParameters(Eigen::MatrixX4d &camera_chain_dh_offsets,
+                                                   Eigen::MatrixX4d &target_chain_dh_offsets,
+                                                   Eigen::Vector3d &camera_mount_to_camera_position,
+                                                   Eigen::Vector3d &camera_mount_to_camera_angle_axis,
+                                                   Eigen::Vector3d &target_mount_to_target_position,
+                                                   Eigen::Vector3d &target_mount_to_target_angle_axis,
+                                                   Eigen::Vector3d &camera_chain_base_to_target_chain_base_position,
+                                                   Eigen::Vector3d &camera_chain_base_to_target_chain_base_angle_axis)
+  {
+    std::vector<double *> parameters;
+    parameters.push_back(camera_chain_dh_offsets.data());
+    parameters.push_back(target_chain_dh_offsets.data());
+    parameters.push_back(camera_mount_to_camera_position.data());
+    parameters.push_back(camera_mount_to_camera_angle_axis.data());
+    parameters.push_back(target_mount_to_target_position.data());
+    parameters.push_back(target_mount_to_target_angle_axis.data());
+    parameters.push_back(camera_chain_base_to_target_chain_base_position.data());
+    parameters.push_back(camera_chain_base_to_target_chain_base_angle_axis.data());
+    return parameters;
+  }
+
+  template<typename T>
+  bool operator()(T const *const *parameters, T *residual) const
+  {
+    // Step 1: Load the data
+    // The first parameter is a pointer to the DH parameter offsets of the camera kinematic chain
+    Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, 4>> camera_chain_dh_offsets(parameters[0], camera_chain_.dof(), 4);
+
+    // The next parameter is a pointer to the DH parameter offsets of the target kinematic chain
+    Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, 4>> target_chain_dh_offsets(parameters[1], target_chain_.dof(), 4);
+
+    // The next two parameters are pointers to the position and angle axis of the transform from the camera mount to the camera
+    std::size_t cm_to_c_idx = 2;
+    const Isometry3<T> camera_mount_to_camera = createTransform(parameters, cm_to_c_idx);
+
+    // The next two parameters are pointers to the position and angle axis of the transform from the target mount to the target
+    std::size_t tm_to_t_idx = cm_to_c_idx + 2;
+    const Isometry3<T> target_mount_to_target = createTransform(parameters, tm_to_t_idx);
+
+    // The next two parameters are pointers to the position and angle axis of the transform from the camera chain base to the target chain base
+    std::size_t cb_to_tb_idx = tm_to_t_idx + 2;
+    const Isometry3<T> camera_base_to_target_base = createTransform(parameters, cb_to_tb_idx);
+
+    // Step 2: Transformation math
+    // Build the transforms from the camera chain base out to the camera
+    Isometry3<T> camera_chain_fk = camera_chain_.getFK<T>(camera_chain_joints_.cast<T>(),
+                                                          camera_chain_dh_offsets);
+    Isometry3<T> camera_base_to_camera = camera_chain_fk * camera_mount_to_camera;
+
+    // Build the transforms from the camera chain base out to the target
+    Isometry3<T> target_chain_fk = target_chain_.getFK<T>(target_chain_joints_.cast<T>(),
+                                                          target_chain_dh_offsets);
+    Isometry3<T> camera_base_to_target = camera_base_to_target_base * target_chain_fk
+                                         * target_mount_to_target;
+
+    // Now that we have two transforms in the same frame, get the target point in the camera frame
+    Isometry3<T> camera_to_target = camera_base_to_camera.inverse() * camera_base_to_target;
+    Vector3<T> target_in_camera = camera_to_target * target_pt_.cast<T>();
+
+    // Project the target into the image plane
+    Vector2<T> target_in_image = projectPoint(intr_, target_in_camera);
+
+    // Step 3: Calculate the error
+    residual[0] = target_in_image.x() - obs_.x();
+    residual[1] = target_in_image.y() - obs_.y();
+
+    return true;
+  }
+
+  protected:
+  Eigen::Vector2d obs_;
+  Eigen::Vector3d target_pt_;
+  CameraIntrinsics intr_;
+
+  const DHChain &camera_chain_;
+  const DHChain &target_chain_;
+
+  Eigen::VectorXd camera_chain_joints_;
+  Eigen::VectorXd target_chain_joints_;
+};
+
+KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D &problem);
+
+} // namespace rct_optimizations
+