Dual DH Chain Kinematic Calibration Update (#72)

* Added function to create new chain from modified DH parameters

* Add likelihood cost to DH chain kinematic calibration problem

* Update DH chain kinematic calibration unit test

* Added subset parameterization

* Added local parameterization unit test

* Fixed typo in utility function name

* Added mask variable to kinematic calibration problem struct

* Added subset parameterization to kinematic calibration

* Updated unit test for DH chain calibration

rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h

@@ -7,6 +7,32 @@
 
 namespace rct_optimizations
 {
+/**
+   * @brief Create a mask of parameter indices from a matrix of boolean values
+   * The indices are calculated in column-wise order because Eigen stores it's values internally in column-wise order by default
+   * @param mask
+   * @return
+   */
+inline std::vector<int> createDHMask(const Eigen::Array<bool, Eigen::Dynamic, 4>& mask)
+{
+  std::vector<int> out;
+  out.reserve(mask.size());
+
+  const Eigen::Index rows = mask.rows();
+  for (Eigen::Index row = 0; row < mask.rows(); ++row)
+  {
+    for (Eigen::Index col = 0; col < mask.cols(); ++col)
+    {
+      if (mask(row, col))
+      {
+        out.push_back(rows * col + row);
+      }
+    }
+  }
+
+  return out;
+}
+
 struct KinematicCalibrationProblem2D3D
 {
   KinematicCalibrationProblem2D3D(DHChain camera_chain_, DHChain target_chain_)
@@ -18,14 +44,28 @@ struct KinematicCalibrationProblem2D3D
   {
   }
 
-  DHChain camera_chain;
-  DHChain target_chain;
   KinObservation2D3D::Set observations;
   CameraIntrinsics intr;
+
+  // Optimization Variables
+  DHChain camera_chain;
+  DHChain target_chain;
   Eigen::Isometry3d camera_mount_to_camera_guess;
   Eigen::Isometry3d target_mount_to_target_guess;
   Eigen::Isometry3d camera_base_to_target_base_guess;
 
+  /* Create an array of masks
+   * 0. Camera DH parameters (size joints x 4)
+   * 1. Target DH parameters (size joints x 4)
+   * 2. Camera mount to camera position (size 3)
+   * 3. Camera mount to camera angle axis (size 3)
+   * 4. Target mount to target position (size 3)
+   * 5. Target mount to target angle axis (size 3)
+   * 6. Camera base to target base position (size 3)
+   * 7. Target mount to target base angle axis (size 3)
+   */
+  std::array<std::vector<int>, 8> mask;
+
   std::string label_camera_mount_to_camera = "camera_mount_to_camera";
   std::string label_target_mount_to_target = "target_mount_to_target";
   std::string label_camera_base_to_target = "camera_base_to_target";
@@ -36,13 +76,13 @@ 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
   CovarianceResult covariance;
 };
 

rct_optimizations/include/rct_optimizations/local_parameterization.h

@@ -1,7 +1,9 @@
 #pragma once
 
-#include <ceres/rotation.h>
-#include <Eigen/Dense>
+#include <Eigen/Core>
+#include <ceres/problem.h>
+#include <ceres/local_parameterization.h>
+#include <rct_optimizations/types.h>
 
 // Ceres Solver - A fast non-linear least squares minimizer
 // Copyright 2015 Google Inc. All rights reserved.
@@ -73,4 +75,70 @@ 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
+ * @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)
+{
+  // 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 (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());
+  }
+
+  // Set identity local parameterization on individual variables that we want to remain constant
+  for (std::size_t i = 0; i < parameter_blocks.size(); ++i)
+  {
+    std::size_t block_size = problem.ParameterBlockSize(parameter_blocks.at(i));
+    std::vector<int> mask = masks.at(i);
+
+    if (mask.size() > 0)
+    {
+      // 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());
+
+      // 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(parameter_blocks.at(i));
+      }
+      else
+      {
+        ceres::LocalParameterization* lp = new ceres::SubsetParameterization(block_size, mask);
+        problem.SetParameterization(parameter_blocks[i], lp);
+      }
+    }
+  }
+}
+
 } // namespace rct_optimizations

rct_optimizations/src/rct_optimizations/dh_chain_kinematic_calibration.cpp

@@ -2,6 +2,8 @@
 #include <rct_optimizations/ceres_math_utilities.h>
 #include <rct_optimizations/eigen_conversions.h>
 #include <rct_optimizations/covariance_analysis.h>
+#include <rct_optimizations/maximum_likelihood.h>
+#include <rct_optimizations/local_parameterization.h>
 
 #include <ceres/ceres.h>
 
@@ -24,7 +26,7 @@ Eigen::Isometry3d createTransform(const Eigen::Vector3d& t, const Eigen::Vector3
 KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D &params)
 {
   // Initialize the optimization variables
-  // Camera mount to camera (cm_to_c) quaternion and translation
+  // Camera mount to camera (cm_to_c) unnormalized angle axis and translation
   Eigen::Vector3d t_cm_to_c(params.camera_mount_to_camera_guess.translation());
   Eigen::AngleAxisd rot_cm_to_c(params.camera_mount_to_camera_guess.rotation());
   Eigen::Vector3d aa_cm_to_c(rot_cm_to_c.angle() * rot_cm_to_c.axis());
@@ -160,17 +162,52 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D &param
   if (params.target_chain.dof() == 0)
     problem.SetParameterBlockConstant(target_chain_dh_offsets.data());
 
-  // TODO: Set bounds on the DH parameter offsets
-//  problem.SetParameterUpperBound(camera_chain_dh_offsets.data(), 0, 0.01);
-//  problem.SetParameterLowerBound(camera_chain_dh_offsets.data(), 0, -0.01);
+  // Add subset parameterization to mask variables that shouldn't be optimized
+  std::array<double*, 8> tmp;
+  std::copy_n(parameters.begin(), tmp.size(), tmp.begin());
+  addSubsetParameterization(problem, params.mask, tmp);
 
-  // TODO: Set identity local parameterization on individual variables that we want to remain constant
-  // This will likely require an additional argument
+  // Add a cost to drive the camera chain DH parameters towards an expected mean
+  if (params.camera_chain.dof() != 0)
+  {
+    Eigen::ArrayXXd mean(
+      Eigen::ArrayXXd::Zero(camera_chain_dh_offsets.rows(), camera_chain_dh_offsets.cols()));
+
+    Eigen::ArrayXXd stdev(Eigen::ArrayXXd::Constant(camera_chain_dh_offsets.rows(),
+                                                    camera_chain_dh_offsets.cols(),
+                                                    1.0e-3));
+
+    auto *fn = new MaximumLikelihood(mean, stdev);
+    auto *cost_block = new ceres::DynamicAutoDiffCostFunction<MaximumLikelihood>(fn);
+    cost_block->AddParameterBlock(camera_chain_dh_offsets.size());
+    cost_block->SetNumResiduals(camera_chain_dh_offsets.size());
+
+    problem.AddResidualBlock(cost_block, nullptr, camera_chain_dh_offsets.data());
+  }
+
+  // Add a cost to drive the target chain DH parameters towards an expected mean
+  if (params.target_chain.dof() != 0)
+  {
+    Eigen::ArrayXXd mean(
+      Eigen::ArrayXXd::Zero(target_chain_dh_offsets.rows(), target_chain_dh_offsets.cols()));
+
+    Eigen::ArrayXXd stdev(Eigen::ArrayXXd::Constant(target_chain_dh_offsets.rows(),
+                                                   target_chain_dh_offsets.cols(),
+                                                   1.0e-3));
+
+    auto *fn = new MaximumLikelihood(mean, stdev);
+    auto *cost_block = new ceres::DynamicAutoDiffCostFunction<MaximumLikelihood>(fn);
+    cost_block->AddParameterBlock(target_chain_dh_offsets.size());
+    cost_block->SetNumResiduals(target_chain_dh_offsets.size());
+
+    problem.AddResidualBlock(cost_block, nullptr, target_chain_dh_offsets.data());
+  }
 
   // Setup the Ceres optimization parameters
   ceres::Solver::Options options;
   options.max_num_iterations = 150;
   options.num_threads = 4;
+  options.minimizer_progress_to_stdout = true;
   ceres::Solver::Summary summary;
 
   // Solve the optimization

rct_optimizations/test/CMakeLists.txt

@@ -95,6 +95,13 @@ rct_gtest_discover_tests(${PROJECT_NAME}_dh_chain_kinematic_calibration_tests)
 add_dependencies(${PROJECT_NAME}_dh_chain_kinematic_calibration_tests ${PROJECT_NAME})
 add_dependencies(run_tests ${PROJECT_NAME}_dh_chain_kinematic_calibration_tests)
 
+# Local Parameterization
+add_executable(${PROJECT_NAME}_local_parameterization_tests local_parameterization_utest.cpp)
+target_link_libraries(${PROJECT_NAME}_local_parameterization_tests PRIVATE ${PROJECT_NAME}_test_support GTest::GTest GTest::Main)
+rct_gtest_discover_tests(${PROJECT_NAME}_local_parameterization_tests)
+add_dependencies(${PROJECT_NAME}_local_parameterization_tests ${PROJECT_NAME})
+add_dependencies(run_tests ${PROJECT_NAME}_local_parameterization_tests)
+
 # Install the test executables so they can be run independently later if needed
 install(
   TARGETS
@@ -110,6 +117,7 @@ install(
     ${PROJECT_NAME}_noise_tests
     ${PROJECT_NAME}_maximum_likelihood_tests
     ${PROJECT_NAME}_dh_chain_kinematic_calibration_tests
+    ${PROJECT_NAME}_local_parameterization_tests
   RUNTIME DESTINATION bin/tests
   LIBRARY DESTINATION lib/tests
   ARCHIVE DESTINATION lib/tests