Ported changes from Jmeyer1292/robot_cal_tools#99

include/industrial_calibration/cost_functions/dh_chain_kinematic_calibration.h

@@ -5,6 +5,7 @@
 #include <industrial_calibration/optimizations/utils/ceres_math_utilities.h>
 
 #include <ceres/fpclassify.h>
+#include <map>
 
 namespace industrial_calibration
 {
@@ -62,46 +63,78 @@ class DualDHChainCost
     return parameters;
   }
 
-  static std::vector<std::vector<std::string>>
-  constructParameterLabels(const std::vector<std::array<std::string, 4>>& camera_chain_labels,
-                           const std::vector<std::array<std::string, 4>>& target_chain_labels,
-                           const std::array<std::string, 3>& camera_mount_to_camera_position_labels,
-                           const std::array<std::string, 3>& camera_mount_to_camera_angle_axis_labels,
-                           const std::array<std::string, 3>& target_mount_to_target_position_labels,
-                           const std::array<std::string, 3>& target_mount_to_target_angle_axis_labels,
-                           const std::array<std::string, 3>& camera_chain_base_to_target_chain_base_position_labels,
-                           const std::array<std::string, 3>& camera_chain_base_to_target_chain_base_angle_axis_labels)
+  static std::map<const double*, std::vector<std::string>> constructParameterLabels(
+      const std::vector<double*>& parameters, const std::vector<std::array<std::string, 4>>& camera_chain_labels,
+      const std::vector<std::array<std::string, 4>>& target_chain_labels,
+      const std::array<std::string, 3>& camera_mount_to_camera_position_labels,
+      const std::array<std::string, 3>& camera_mount_to_camera_angle_axis_labels,
+      const std::array<std::string, 3>& target_mount_to_target_position_labels,
+      const std::array<std::string, 3>& target_mount_to_target_angle_axis_labels,
+      const std::array<std::string, 3>& camera_chain_base_to_target_chain_base_position_labels,
+      const std::array<std::string, 3>& camera_chain_base_to_target_chain_base_angle_axis_labels)
   {
-    std::vector<std::vector<std::string>> param_labels;
+    // Eigen is column major so need to store labels column major
+    std::map<const double*, std::vector<std::string>> param_labels;
     std::vector<std::string> cc_labels_concatenated;
-    for (auto cc_label : camera_chain_labels)
+    for (std::size_t c = 0; c < 4; ++c)
     {
-      cc_labels_concatenated.insert(cc_labels_concatenated.end(), cc_label.begin(), cc_label.end());
+      for (auto cc_label : camera_chain_labels)
+        cc_labels_concatenated.push_back(cc_label.at(c));
     }
-    param_labels.push_back(cc_labels_concatenated);
+    param_labels[parameters[0]] = cc_labels_concatenated;
 
+    // Eigen is column major so need to store labels column major
     std::vector<std::string> tc_labels_concatenated;
-    for (auto tc_label : target_chain_labels)
+    for (std::size_t c = 0; c < 4; ++c)
     {
-      tc_labels_concatenated.insert(tc_labels_concatenated.end(), tc_label.begin(), tc_label.end());
+      for (auto tc_label : target_chain_labels)
+        tc_labels_concatenated.push_back(tc_label.at(c));
     }
-    param_labels.push_back(tc_labels_concatenated);
-
-    param_labels.emplace_back(camera_mount_to_camera_position_labels.begin(),
-                              camera_mount_to_camera_position_labels.end());
-    param_labels.emplace_back(camera_mount_to_camera_angle_axis_labels.begin(),
-                              camera_mount_to_camera_angle_axis_labels.end());
-    param_labels.emplace_back(target_mount_to_target_position_labels.begin(),
-                              target_mount_to_target_position_labels.end());
-    param_labels.emplace_back(target_mount_to_target_angle_axis_labels.begin(),
-                              target_mount_to_target_angle_axis_labels.end());
-    param_labels.emplace_back(camera_chain_base_to_target_chain_base_position_labels.begin(),
-                              camera_chain_base_to_target_chain_base_position_labels.end());
-    param_labels.emplace_back(camera_chain_base_to_target_chain_base_angle_axis_labels.begin(),
-                              camera_chain_base_to_target_chain_base_angle_axis_labels.end());
+    param_labels[parameters[1]] = tc_labels_concatenated;
+
+    param_labels[parameters[2]] = std::vector<std::string>(camera_mount_to_camera_position_labels.begin(),
+                                                           camera_mount_to_camera_position_labels.end());
+    param_labels[parameters[3]] = std::vector<std::string>(camera_mount_to_camera_angle_axis_labels.begin(),
+                                                           camera_mount_to_camera_angle_axis_labels.end());
+    param_labels[parameters[4]] = std::vector<std::string>(target_mount_to_target_position_labels.begin(),
+                                                           target_mount_to_target_position_labels.end());
+    param_labels[parameters[5]] = std::vector<std::string>(target_mount_to_target_angle_axis_labels.begin(),
+                                                           target_mount_to_target_angle_axis_labels.end());
+    param_labels[parameters[6]] =
+        std::vector<std::string>(camera_chain_base_to_target_chain_base_position_labels.begin(),
+                                 camera_chain_base_to_target_chain_base_position_labels.end());
+    param_labels[parameters[7]] =
+        std::vector<std::string>(camera_chain_base_to_target_chain_base_angle_axis_labels.begin(),
+                                 camera_chain_base_to_target_chain_base_angle_axis_labels.end());
     return param_labels;
   }
 
+  static std::map<const double*, std::vector<int>> constructParameterMasks(const std::vector<double*>& parameters,
+                                                                           const std::array<std::vector<int>, 8>& masks)
+  {
+    assert(parameters.size() == masks.size());
+    std::map<const double*, std::vector<int>> param_masks;
+    for (std::size_t i = 0; i < parameters.size(); ++i)
+      param_masks[parameters[i]] = masks[i];
+
+    return param_masks;
+  }
+
+  static std::map<const double*, std::string> constructParameterNames(const std::vector<double*>& parameters)
+  {
+    std::map<const double*, std::string> names;
+    names[parameters[0]] = "Camera DH Parameters";
+    names[parameters[1]] = "Target DH Parameters";
+    names[parameters[2]] = "Camera Mount to Camera Position Parameters";
+    names[parameters[3]] = "Camera Mount to Camera Orientation Parameters";
+    names[parameters[4]] = "Target Mount to Target Position Parameters";
+    names[parameters[5]] = "Target Mount to Target Orientation Parameters";
+    names[parameters[6]] = "Camera Chain Base to Target Chain Base Position Parameters";
+    names[parameters[7]] = "Camera Chain Base to Target Chain Base Orientation Parameters";
+
+    return names;
+  }
+
 protected:
   const DHChain& camera_chain_;
   const DHChain& target_chain_;

include/industrial_calibration/dh_chain.h

@@ -231,6 +231,14 @@ class DHChain
    */
   Eigen::Isometry3d getBaseOffset() const;
 
+  /**
+   * @brief Get a joints relative joint transform
+   * @param joint_index The joint index
+   * @param value The joint value
+   * @return The joint relative transform
+   */
+  Eigen::Isometry3d getRelativeTransform(int joint_index, double value) const;
+
 protected:
   /** @brief The DH transforms that make up the chain */
   std::vector<DHTransform> transforms_;

include/industrial_calibration/optimizations/utils/covariance_analysis.h

@@ -20,6 +20,15 @@ struct DefaultCovarianceOptions : ceres::Covariance::Options
   }
 };
 
+/**
+ * @brief Get the covariance results provided covariance matrix and parameter names
+ * @param cov_matrix The covariance matrix
+ * @param parameter_names The parameter names
+ * @return The covariance results
+ */
+CovarianceResult computeCovarianceResults(const Eigen::MatrixXd& cov_matrix,
+                                          const std::vector<std::string>& parameter_names);
+
 /**
  * @brief Given a covariance matrix, calculate the matrix of standard deviations and correlation coefficients.
  * @param Covariance matrix
@@ -31,40 +40,53 @@ Eigen::MatrixXd computeCorrelationsFromCovariance(const Eigen::MatrixXd& covaria
 /**
  * @brief Compute all covariance results for a Ceres optimization problem. Labels results with generic names.
  * @param problem The Ceres problem (after optimization).
+ * @param param_masks Map of the parameter block pointer and the indices of the parameters within that block to be
+ * excluded from the covariance calculation
  * @param options ceres::Covariance::Options to use when calculating covariance.
  * @return CovarianceResult for the problem.
  */
-CovarianceResult computeCovariance(ceres::Problem& problem,
-                                   const ceres::Covariance::Options& options = DefaultCovarianceOptions());
+CovarianceResult computeCovariance(
+    ceres::Problem& problem,
+    const std::map<const double*, std::vector<int>>& param_masks = std::map<const double*, std::vector<int>>(),
+    const ceres::Covariance::Options& options = DefaultCovarianceOptions());
 
 /**
  * @brief Compute covariance results for the specified parameter blocks in a Ceres optimization problem. Labels results
  * with generic names.
  * @param problem The Ceres problem (after optimization).
  * @param parameter_blocks Specific parameter blocks to compute covariance between.
+ * @param param_masks Map of the parameter block pointer and the indices of the parameters within that block to be
+ * excluded from the covariance calculation
  * @param options ceres::Covariance::Options to use when calculating covariance.
  * @return CovarianceResult for the problem.
  */
-CovarianceResult computeCovariance(ceres::Problem& problem, const std::vector<const double*>& parameter_blocks,
-                                   const ceres::Covariance::Options& options = DefaultCovarianceOptions());
+CovarianceResult computeCovariance(
+    ceres::Problem& problem, const std::vector<const double*>& parameter_blocks,
+    const std::map<const double*, std::vector<int>>& param_masks = std::map<const double*, std::vector<int>>(),
+    const ceres::Covariance::Options& options = DefaultCovarianceOptions());
 
 /**
  * @brief Compute all covariance results for a Ceres optimization problem and label them with the provided names.
  * @param problem The Ceres problem (after optimization).
  * @param parameter_names Labels for all optimization parameters in the problem.
+ * @param param_masks Map of the parameter block pointer and the indices of the parameters within that block to be
+ * excluded from the covariance calculation
  * @param options ceres::Covariance::Options to use when calculating covariance.
  * @return CovarianceResult for the problem.
  */
-CovarianceResult computeCovariance(ceres::Problem& problem,
-                                   const std::vector<std::vector<std::string>>& parameter_names,
-                                   const ceres::Covariance::Options& options = DefaultCovarianceOptions());
+CovarianceResult computeCovariance(
+    ceres::Problem& problem, const std::map<const double*, std::vector<std::string>>& param_names,
+    const std::map<const double*, std::vector<int>>& param_masks = std::map<const double*, std::vector<int>>(),
+    const ceres::Covariance::Options& options = DefaultCovarianceOptions());
 
 /**
  * @brief Compute covariance results for specified parameter blocks in a Ceres optimization problem and label them with
  * the provided names.
  * @param problem The Ceres problem (after optimization).
  * @param parameter_blocks Specific parameter blocks for which covariance will be calculated.
  * @param parameter_names Labels for optimization parameters in the specified blocks.
+ * @param param_masks Map of the parameter block pointer and the indices of the parameters within that block to be
+ * excluded from the covariance calculation
  * @param options ceres::Covariance::Options to use when calculating covariance.
  * @return CovarianceResult for the problem.
  * @throw CovarianceException if covariance.Compute fails.
@@ -73,7 +95,10 @@ CovarianceResult computeCovariance(ceres::Problem& problem,
  * @throw CovarianceException if the number of parameter label strings provided for a block is different than the number
  * of parameters in that block.
  */
-CovarianceResult computeCovariance(ceres::Problem& problem, const std::vector<const double*>& parameter_blocks,
-                                   const std::vector<std::vector<std::string>>& parameter_names,
-                                   const ceres::Covariance::Options& options = DefaultCovarianceOptions());
+CovarianceResult computeCovariance(
+    ceres::Problem& problem, const std::vector<const double*>& parameter_blocks,
+    const std::map<const double*, std::vector<std::string>>& param_names,
+    const std::map<const double*, std::vector<int>>& param_masks = std::map<const double*, std::vector<int>>(),
+    const ceres::Covariance::Options& options = DefaultCovarianceOptions());
+
 }  // namespace industrial_calibration

include/industrial_calibration/optimizations/utils/local_parameterization.h

@@ -81,64 +81,55 @@ struct EigenQuaternionPlus
 /**
  * @brief Adds subset parameterization for all parameter blocks for a given problem
  * @param problem - Ceres optimization problem
- * @param masks - an array of masks indicating the index of the parameters that should be held constant.
- *   - The array must be the same size as the number of parameter blocks in the problem
- *   - Individual mask vectors cannot mask all parameters in a block
- * @param parameter_blocks - Ceres parameter blocks in the same order as the input masks
+ * @param param_masks - A map of parameter to an array of masks indicating the index of the parameters that should be
+ * held constant.
+ *   - The map must be the same size as the number of parameter blocks in the problem
+ *   - If all parameters are masked it sets that block to constant
  * @throws OptimizationException
  */
-template <std::size_t N_BLOCKS>
-void addSubsetParameterization(ceres::Problem& problem, const std::array<std::vector<int>, N_BLOCKS>& masks,
-                               const std::array<double*, N_BLOCKS>& parameter_blocks)
+void addSubsetParameterization(ceres::Problem& problem, const std::map<const double*, std::vector<int>>& param_masks)
 {
-  // Make sure the Ceres problem has the same number of blocks as the input
-  if (problem.NumParameterBlocks() != N_BLOCKS)
-  {
-    std::stringstream ss;
-    ss << "Input parameter block size does not match Ceres problem parameter block size (" << N_BLOCKS
-       << " != " << problem.NumParameterBlocks() << ")";
-    throw OptimizationException(ss.str());
-  }
+  if (param_masks.empty()) return;
 
-  if (parameter_blocks.size() != masks.size())
-  {
-    std::stringstream ss;
-    ss << "Parameter block count does not match number of masks (" << parameter_blocks.size() << " != " << masks.size();
-    throw OptimizationException(ss.str());
-  }
+  std::vector<double*> parameter_blocks;
+  problem.GetParameterBlocks(&parameter_blocks);
 
   // Set identity local parameterization on individual variables that we want to remain constant
-  for (std::size_t i = 0; i < parameter_blocks.size(); ++i)
+  for (const auto& p : parameter_blocks)
   {
-    std::size_t block_size = problem.ParameterBlockSize(parameter_blocks.at(i));
-    std::vector<int> mask = masks.at(i);
-
-    if (mask.size() > 0)
+    std::size_t block_size = problem.ParameterBlockSize(p);
+    auto it = param_masks.find(p);
+    if (it != param_masks.end())
     {
-      // Sort the array and remove duplicate indices
-      std::sort(mask.begin(), mask.end());
-      auto last = std::unique(mask.begin(), mask.end());
-      mask.erase(last, mask.end());
+      std::vector<int> mask = it->second;
 
-      // Check that the max index is not greater than the number of elements in the block
-      auto it = std::max_element(mask.begin(), mask.end());
-      if (static_cast<std::size_t>(*it) >= block_size)
+      if (mask.size() > 0)
       {
-        std::stringstream ss;
-        ss << "The largest mask index cannot be larger than or equal to the parameter block size (" << *it
-           << " >= " << block_size << ")";
-        throw OptimizationException(ss.str());
-      }
+        // Sort the array and remove duplicate indices
+        std::sort(mask.begin(), mask.end());
+        auto last = std::unique(mask.begin(), mask.end());
+        mask.erase(last, mask.end());
 
-      // Set local parameterization on the indices or set the entire block constant
-      if (mask.size() >= block_size)
-      {
-        problem.SetParameterBlockConstant(parameter_blocks.at(i));
-      }
-      else
-      {
-        ceres::LocalParameterization* lp = new ceres::SubsetParameterization(block_size, mask);
-        problem.SetParameterization(parameter_blocks[i], lp);
+        // Check that the max index is not greater than the number of elements in the block
+        auto it = std::max_element(mask.begin(), mask.end());
+        if (static_cast<std::size_t>(*it) >= block_size)
+        {
+          std::stringstream ss;
+          ss << "The largest mask index cannot be larger than or equal to the parameter block size (" << *it
+             << " >= " << block_size << ")";
+          throw OptimizationException(ss.str());
+        }
+
+        // Set local parameterization on the indices or set the entire block constant
+        if (mask.size() >= block_size)
+        {
+          problem.SetParameterBlockConstant(p);
+        }
+        else
+        {
+          ceres::LocalParameterization* lp = new ceres::SubsetParameterization(block_size, mask);
+          problem.SetParameterization(p, lp);
+        }
       }
     }
   }

include/industrial_calibration/types.h

@@ -11,7 +11,7 @@ namespace industrial_calibration
  */
 struct CameraIntrinsics
 {
-  std::array<double, 4> values;
+  std::array<double, 4> values{ 0.0, 0.0, 0.0, 0.0 };
 
   double& fx()
   {
@@ -207,7 +207,7 @@ struct Pose6d
   {
   }
 
-  std::array<double, 6> values;
+  std::array<double, 6> values{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
 
   double& rx()
   {

src/dh_chain.cpp

@@ -113,4 +113,12 @@ Eigen::Isometry3d DHChain::getBaseOffset() const
   return base_offset_;
 }
 
+Eigen::Isometry3d DHChain::getRelativeTransform(int joint_index, double value) const
+{
+  if (joint_index < 0 || joint_index >= static_cast<int>(transforms_.size()))
+    throw std::runtime_error("getRelativeTransform, Invalid joint index");
+
+  return transforms_[joint_index].createRelativeTransform(value);
+}
+
 }  // namespace industrial_calibration

src/optimizations/camera_intrinsic.cpp

@@ -76,10 +76,11 @@ CameraIntrinsicResult optimize(const CameraIntrinsicProblem& params)
   }
 
   std::vector<const double*> param_blocks;
-  std::vector<std::vector<std::string>> param_labels;
+  std::map<const double*, std::vector<std::string>> param_labels;
 
   param_blocks.emplace_back(internal_intrinsics_data.data());
-  param_labels.emplace_back(params.labels_intrinsic_params.begin(), params.labels_intrinsic_params.end());
+  param_labels[internal_intrinsics_data.data()] =
+      std::vector<std::string>(params.labels_intrinsic_params.begin(), params.labels_intrinsic_params.end());
 
   for (std::size_t i = 0; i < internal_poses.size(); i++)
   {
@@ -90,7 +91,7 @@ CameraIntrinsicResult optimize(const CameraIntrinsicProblem& params)
     {
       labels.push_back(params.label_extr + std::to_string(i) + "_" + label_extr);
     }
-    param_labels.push_back(labels);
+    param_labels[internal_poses[i].values.data()] = labels;
   }
 
   // Solve