diff --git a/include/industrial_calibration/cost_functions/dh_chain_kinematic_calibration.h b/include/industrial_calibration/cost_functions/dh_chain_kinematic_calibration.h index 6c14aeec..b2b92b88 100644 --- a/include/industrial_calibration/cost_functions/dh_chain_kinematic_calibration.h +++ b/include/industrial_calibration/cost_functions/dh_chain_kinematic_calibration.h @@ -5,6 +5,7 @@ #include #include +#include namespace industrial_calibration { @@ -62,46 +63,78 @@ class DualDHChainCost return parameters; } - static std::vector> - constructParameterLabels(const std::vector>& camera_chain_labels, - const std::vector>& target_chain_labels, - const std::array& camera_mount_to_camera_position_labels, - const std::array& camera_mount_to_camera_angle_axis_labels, - const std::array& target_mount_to_target_position_labels, - const std::array& target_mount_to_target_angle_axis_labels, - const std::array& camera_chain_base_to_target_chain_base_position_labels, - const std::array& camera_chain_base_to_target_chain_base_angle_axis_labels) + static std::map> constructParameterLabels( + const std::vector& parameters, const std::vector>& camera_chain_labels, + const std::vector>& target_chain_labels, + const std::array& camera_mount_to_camera_position_labels, + const std::array& camera_mount_to_camera_angle_axis_labels, + const std::array& target_mount_to_target_position_labels, + const std::array& target_mount_to_target_angle_axis_labels, + const std::array& camera_chain_base_to_target_chain_base_position_labels, + const std::array& camera_chain_base_to_target_chain_base_angle_axis_labels) { - std::vector> param_labels; + // Eigen is column major so need to store labels column major + std::map> param_labels; std::vector 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 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(camera_mount_to_camera_position_labels.begin(), + camera_mount_to_camera_position_labels.end()); + param_labels[parameters[3]] = std::vector(camera_mount_to_camera_angle_axis_labels.begin(), + camera_mount_to_camera_angle_axis_labels.end()); + param_labels[parameters[4]] = std::vector(target_mount_to_target_position_labels.begin(), + target_mount_to_target_position_labels.end()); + param_labels[parameters[5]] = std::vector(target_mount_to_target_angle_axis_labels.begin(), + target_mount_to_target_angle_axis_labels.end()); + param_labels[parameters[6]] = + std::vector(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(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> constructParameterMasks(const std::vector& parameters, + const std::array, 8>& masks) + { + assert(parameters.size() == masks.size()); + std::map> param_masks; + for (std::size_t i = 0; i < parameters.size(); ++i) + param_masks[parameters[i]] = masks[i]; + + return param_masks; + } + + static std::map constructParameterNames(const std::vector& parameters) + { + std::map 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_; diff --git a/include/industrial_calibration/dh_chain.h b/include/industrial_calibration/dh_chain.h index fb72a456..07918c26 100644 --- a/include/industrial_calibration/dh_chain.h +++ b/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 transforms_; diff --git a/include/industrial_calibration/optimizations/utils/covariance_analysis.h b/include/industrial_calibration/optimizations/utils/covariance_analysis.h index fa919148..62f8e064 100644 --- a/include/industrial_calibration/optimizations/utils/covariance_analysis.h +++ b/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& parameter_names); + /** * @brief Given a covariance matrix, calculate the matrix of standard deviations and correlation coefficients. * @param Covariance matrix @@ -31,33 +40,44 @@ 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>& param_masks = std::map>(), + 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& parameter_blocks, - const ceres::Covariance::Options& options = DefaultCovarianceOptions()); +CovarianceResult computeCovariance( + ceres::Problem& problem, const std::vector& parameter_blocks, + const std::map>& param_masks = std::map>(), + 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>& parameter_names, - const ceres::Covariance::Options& options = DefaultCovarianceOptions()); +CovarianceResult computeCovariance( + ceres::Problem& problem, const std::map>& param_names, + const std::map>& param_masks = std::map>(), + const ceres::Covariance::Options& options = DefaultCovarianceOptions()); /** * @brief Compute covariance results for specified parameter blocks in a Ceres optimization problem and label them with @@ -65,6 +85,8 @@ CovarianceResult computeCovariance(ceres::Problem& problem, * @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& parameter_blocks, - const std::vector>& parameter_names, - const ceres::Covariance::Options& options = DefaultCovarianceOptions()); +CovarianceResult computeCovariance( + ceres::Problem& problem, const std::vector& parameter_blocks, + const std::map>& param_names, + const std::map>& param_masks = std::map>(), + const ceres::Covariance::Options& options = DefaultCovarianceOptions()); + } // namespace industrial_calibration diff --git a/include/industrial_calibration/optimizations/utils/local_parameterization.h b/include/industrial_calibration/optimizations/utils/local_parameterization.h index a200e85e..8385d90f 100644 --- a/include/industrial_calibration/optimizations/utils/local_parameterization.h +++ b/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 -void addSubsetParameterization(ceres::Problem& problem, const std::array, N_BLOCKS>& masks, - const std::array& parameter_blocks) +void addSubsetParameterization(ceres::Problem& problem, const std::map>& 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 parameter_blocks; + problem.GetParameterBlocks(¶meter_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 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 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(*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(*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); + } } } } diff --git a/include/industrial_calibration/types.h b/include/industrial_calibration/types.h index c3d3c00c..98ad47ee 100644 --- a/include/industrial_calibration/types.h +++ b/include/industrial_calibration/types.h @@ -11,7 +11,7 @@ namespace industrial_calibration */ struct CameraIntrinsics { - std::array values; + std::array values{ 0.0, 0.0, 0.0, 0.0 }; double& fx() { @@ -207,7 +207,7 @@ struct Pose6d { } - std::array values; + std::array values{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; double& rx() { diff --git a/src/dh_chain.cpp b/src/dh_chain.cpp index 0fbe9efa..2226ed9d 100644 --- a/src/dh_chain.cpp +++ b/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(transforms_.size())) + throw std::runtime_error("getRelativeTransform, Invalid joint index"); + + return transforms_[joint_index].createRelativeTransform(value); +} + } // namespace industrial_calibration diff --git a/src/optimizations/camera_intrinsic.cpp b/src/optimizations/camera_intrinsic.cpp index 01e419a7..81f8d6ec 100644 --- a/src/optimizations/camera_intrinsic.cpp +++ b/src/optimizations/camera_intrinsic.cpp @@ -76,10 +76,11 @@ CameraIntrinsicResult optimize(const CameraIntrinsicProblem& params) } std::vector param_blocks; - std::vector> param_labels; + std::map> 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(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 diff --git a/src/optimizations/dh_chain_kinematic_calibration.cpp b/src/optimizations/dh_chain_kinematic_calibration.cpp index c77b68d4..83a37004 100644 --- a/src/optimizations/dh_chain_kinematic_calibration.cpp +++ b/src/optimizations/dh_chain_kinematic_calibration.cpp @@ -22,6 +22,51 @@ Eigen::Isometry3d createTransform(const Eigen::Vector3d& t, const Eigen::Vector3 return result; } +void printLabels(const std::string& block_name, const std::vector& param_labels) +{ + std::cout << block_name << ":" << std::endl; + if (param_labels.empty()) + { + std::cout << " - Constant" << std::endl; + return; + } + + for (const auto& param_label : param_labels) + std::cout << " - " << param_label << std::endl; +} + +void printOptimizationLabels(ceres::Problem& problem, const std::map& names, + const std::map>& labels, + const std::map>& masks) +{ + std::vector blocks; + problem.GetParameterBlocks(&blocks); + + // Extract optimization labels + for (double* b : blocks) + { + std::vector sub_label; + if (!problem.IsParameterBlockConstant(b)) + { + std::vector label = labels.at(b); + const std::vector& mask = masks.at(b); + if (mask.empty()) + { + sub_label = label; + } + else + { + sub_label.reserve(label.size()); + for (std::size_t j = 0; j < label.size(); ++j) + { + if (std::find(mask.begin(), mask.end(), j) == mask.end()) sub_label.push_back(label.at(j)); + } + } + } + printLabels(names.at(b), sub_label); + } +} + KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D& params) { // Initialize the optimization variables @@ -97,9 +142,14 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D& param DualDHChain2D3DCost::constructParameters(camera_chain_dh_offsets, target_chain_dh_offsets, t_cm_to_c, aa_cm_to_c, t_tm_to_t, aa_tm_to_t, t_ccb_to_tcb, aa_ccb_to_tcb); - std::vector> param_labels = DualDHChain2D3DCost::constructParameterLabels( - camera_chain_param_labels, target_chain_param_labels, t_cm_to_c_labels, aa_cm_to_c_labels, t_tm_to_t_labels, - aa_tm_to_t_labels, t_ccb_to_tcb_labels, aa_ccb_to_tcb_labels); + std::map> param_labels = DualDHChain2D3DCost::constructParameterLabels( + parameters, camera_chain_param_labels, target_chain_param_labels, t_cm_to_c_labels, aa_cm_to_c_labels, + t_tm_to_t_labels, aa_tm_to_t_labels, t_ccb_to_tcb_labels, aa_ccb_to_tcb_labels); + + std::map> param_masks = + DualDHChain2D3DCost::constructParameterMasks(parameters, params.mask); + + std::map param_names = DualDHChain2D3DCost::constructParameterNames(parameters); // Set up the problem ceres::Problem problem; @@ -146,9 +196,7 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D& param if (params.target_chain.dof() == 0) problem.SetParameterBlockConstant(target_chain_dh_offsets.data()); // Add subset parameterization to mask variables that shouldn't be optimized - std::array tmp; - std::copy_n(parameters.begin(), tmp.size(), tmp.begin()); - addSubsetParameterization(problem, params.mask, tmp); + addSubsetParameterization(problem, param_masks); // Add a cost to drive the camera chain DH parameters towards an expected mean if (params.camera_chain.dof() != 0 && !problem.IsParameterBlockConstant(parameters[0])) @@ -182,6 +230,9 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D& param problem.AddResidualBlock(cost_block, nullptr, target_chain_dh_offsets.data()); } + // Print optimization parameter labels + printOptimizationLabels(problem, param_names, param_labels, param_masks); + // Setup the Ceres optimization parameters ceres::Solver::Options options; options.max_num_iterations = 150; @@ -216,8 +267,7 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D& param ceres::Covariance::Options cov_options = DefaultCovarianceOptions(); cov_options.null_space_rank = -1; // automatically drop terms below min_reciprocal_condition_number - result.covariance = computeCovariance(problem, std::vector(parameters.begin(), parameters.end()), - param_labels, cov_options); + result.covariance = computeCovariance(problem, param_labels, param_masks, cov_options); return result; } @@ -298,9 +348,14 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblemPose6D& par DualDHChainMeasurementCost::constructParameters(camera_chain_dh_offsets, target_chain_dh_offsets, t_cm_to_c, aa_cm_to_c, t_tm_to_t, aa_tm_to_t, t_ccb_to_tcb, aa_ccb_to_tcb); - std::vector> param_labels = DualDHChainMeasurementCost::constructParameterLabels( - camera_chain_param_labels, target_chain_param_labels, t_cm_to_c_labels, aa_cm_to_c_labels, t_tm_to_t_labels, - aa_tm_to_t_labels, t_ccb_to_tcb_labels, aa_ccb_to_tcb_labels); + std::map> param_labels = DualDHChainMeasurementCost::constructParameterLabels( + parameters, camera_chain_param_labels, target_chain_param_labels, t_cm_to_c_labels, aa_cm_to_c_labels, + t_tm_to_t_labels, aa_tm_to_t_labels, t_ccb_to_tcb_labels, aa_ccb_to_tcb_labels); + + std::map> param_masks = + DualDHChainMeasurementCost::constructParameterMasks(parameters, params.mask); + + std::map param_names = DualDHChainMeasurementCost::constructParameterNames(parameters); // Set up the problem ceres::Problem problem; @@ -343,9 +398,7 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblemPose6D& par if (params.target_chain.dof() == 0) problem.SetParameterBlockConstant(target_chain_dh_offsets.data()); // Add subset parameterization to mask variables that shouldn't be optimized - std::array tmp; - std::copy_n(parameters.begin(), tmp.size(), tmp.begin()); - addSubsetParameterization(problem, params.mask, tmp); + addSubsetParameterization(problem, param_masks); // Add a cost to drive the camera chain DH parameters towards an expected mean if (params.camera_chain.dof() != 0 && !problem.IsParameterBlockConstant(parameters[0])) @@ -379,6 +432,9 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblemPose6D& par problem.AddResidualBlock(cost_block, nullptr, target_chain_dh_offsets.data()); } + // Print optimization parameter labels + printOptimizationLabels(problem, param_names, param_labels, param_masks); + // Solve the optimization ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); @@ -407,8 +463,7 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblemPose6D& par ceres::Covariance::Options cov_options = DefaultCovarianceOptions(); cov_options.null_space_rank = -1; // automatically drop terms below min_reciprocal_condition_number - result.covariance = computeCovariance(problem, std::vector(parameters.begin(), parameters.end()), - param_labels, cov_options); + result.covariance = computeCovariance(problem, param_labels, param_masks, cov_options); return result; } diff --git a/src/optimizations/extrinsic_hand_eye.cpp b/src/optimizations/extrinsic_hand_eye.cpp index 5bcc1ee2..1432a783 100644 --- a/src/optimizations/extrinsic_hand_eye.cpp +++ b/src/optimizations/extrinsic_hand_eye.cpp @@ -87,9 +87,9 @@ ExtrinsicHandEyeResult optimize(const ExtrinsicHandEyeProblem2D3D& params) labels_target_mount_to_target.emplace_back(params.label_target_mount_to_target + "_" + label_isometry); } - std::vector> param_labels; - param_labels.push_back(labels_camera_mount_to_camera); - param_labels.push_back(labels_target_mount_to_target); + std::map> param_labels; + param_labels[internal_camera_to_wrist.values.data()] = labels_camera_mount_to_camera; + param_labels[internal_base_to_target.values.data()] = labels_target_mount_to_target; result.covariance = computeCovariance(problem, param_labels); @@ -150,9 +150,9 @@ ExtrinsicHandEyeResult optimize(const ExtrinsicHandEyeProblem3D3D& params) labels_target_mount_to_target.emplace_back(params.label_target_mount_to_target + "_" + label_isometry); } - std::vector> param_labels; - param_labels.push_back(labels_camera_mount_to_camera); - param_labels.push_back(labels_target_mount_to_target); + std::map> param_labels; + param_labels[internal_camera_to_wrist.values.data()] = labels_camera_mount_to_camera; + param_labels[internal_base_to_target.values.data()] = labels_target_mount_to_target; result.covariance = computeCovariance(problem, param_labels); diff --git a/src/optimizations/extrinsic_multi_static_camera.cpp b/src/optimizations/extrinsic_multi_static_camera.cpp index 65073519..f5c02b88 100644 --- a/src/optimizations/extrinsic_multi_static_camera.cpp +++ b/src/optimizations/extrinsic_multi_static_camera.cpp @@ -69,7 +69,7 @@ ExtrinsicMultiStaticCameraMovingTargetResult optimize(const ExtrinsicMultiStatic } param_blocks.push_back(internal_wrist_to_target.values.data()); - std::vector> param_labels; + std::map> param_labels; // compose labels "camera_mount_to_camera_x", etc. for (std::size_t index_camera = 0; index_camera < internal_camera_to_base.size(); index_camera++) @@ -80,7 +80,7 @@ ExtrinsicMultiStaticCameraMovingTargetResult optimize(const ExtrinsicMultiStatic labels_base_to_camera.emplace_back(params.label_base_to_camera + std::to_string(index_camera) + "_" + label_isometry); } - param_labels.push_back(labels_base_to_camera); + param_labels[internal_camera_to_base[index_camera].values.data()] = labels_base_to_camera; } // compose labels "target_mount_to_target_x", etc. @@ -89,7 +89,7 @@ ExtrinsicMultiStaticCameraMovingTargetResult optimize(const ExtrinsicMultiStatic { labels_wrist_to_target.emplace_back(params.label_wrist_to_target + "_" + label_isometry); } - param_labels.push_back(labels_wrist_to_target); + param_labels[internal_wrist_to_target.values.data()] = labels_wrist_to_target; result.covariance = computeCovariance(problem, param_blocks, param_labels); diff --git a/src/optimizations/pnp.cpp b/src/optimizations/pnp.cpp index d5b1767b..47a44d1c 100644 --- a/src/optimizations/pnp.cpp +++ b/src/optimizations/pnp.cpp @@ -51,8 +51,9 @@ PnPResult optimize(const PnPProblem& params) { labels_camera_to_target_guess_quaternion.emplace_back(params.label_camera_to_target_guess + "_" + label_r); } - std::vector> param_labels = { labels_camera_to_target_guess_translation, - labels_camera_to_target_guess_quaternion }; + std::map> param_labels; + param_labels[cam_to_tgt_translation.data()] = labels_camera_to_target_guess_translation; + param_labels[cam_to_tgt_angle_axis.data()] = labels_camera_to_target_guess_quaternion; result.covariance = computeCovariance( problem, std::vector({ cam_to_tgt_translation.data(), cam_to_tgt_angle_axis.data() }), @@ -107,8 +108,9 @@ PnPResult optimize(const PnPProblem3D& params) { labels_camera_to_target_guess_quaternion.emplace_back(params.label_camera_to_target_guess + "_" + label_r); } - std::vector> param_labels = { labels_camera_to_target_guess_translation, - labels_camera_to_target_guess_quaternion }; + std::map> param_labels; + param_labels[cam_to_tgt_translation.data()] = labels_camera_to_target_guess_translation; + param_labels[cam_to_tgt_angle_axis.data()] = labels_camera_to_target_guess_quaternion; result.covariance = computeCovariance( problem, std::vector({ cam_to_tgt_translation.data(), cam_to_tgt_angle_axis.data() }), diff --git a/src/optimizations/utils/covariance_analysis.cpp b/src/optimizations/utils/covariance_analysis.cpp index 2fbdf597..a6b2875f 100644 --- a/src/optimizations/utils/covariance_analysis.cpp +++ b/src/optimizations/utils/covariance_analysis.cpp @@ -37,20 +37,76 @@ Eigen::MatrixXd computeCorrelationsFromCovariance(const Eigen::MatrixXd& covaria return out; } -CovarianceResult computeCovariance(ceres::Problem& problem, const ceres::Covariance::Options& options) +CovarianceResult computeCovarianceResults(const Eigen::MatrixXd& cov_matrix, + const std::vector& parameter_names) +{ + // 0. Save original covariance matrix output + // For parameter blocks [p1, p2], the structure of this matrix will be: + /* | p1 | p2 | + * ---|-----------|-----------| + * p1 | C(p1, p1) | C(p1, p2) | + * p2 | C(p2, p1) | C(p2, p2) | + */ + CovarianceResult res; + res.covariance_matrix = cov_matrix; + + // 1. Compute matrix of standard deviations and correlation coefficients. + // The arrangement of elements in the correlation matrix matches the order in the covariance matrix. + Eigen::MatrixXd correlation_matrix = computeCorrelationsFromCovariance(cov_matrix); + res.correlation_matrix = correlation_matrix; + + // 2. Create NamedParams for covariance and correlation results, which include labels and values for the parameters. + // Uses top-right triangular part of matrix. + Eigen::Index col_start = 0; + for (Eigen::Index row = 0; row < correlation_matrix.rows(); row++) + { + for (Eigen::Index col = col_start; col < correlation_matrix.rows(); col++) + { + NamedParam p_corr; + p_corr.value = correlation_matrix(row, col); + + if (row == col) // diagonal element, standard deviation + { + p_corr.names = std::make_pair(parameter_names[static_cast(row)], ""); + res.standard_deviations.push_back(p_corr); + continue; + } + + // otherwise off-diagonal element, correlation coefficient + p_corr.names = std::make_pair(parameter_names[static_cast(row)], + parameter_names[static_cast(col)]); + res.correlation_coeffs.push_back(p_corr); + + // for off-diagonal elements also get covariance + NamedParam p_cov; + p_cov.value = cov_matrix(row, col); + p_cov.names = std::make_pair(parameter_names[static_cast(row)], + parameter_names[static_cast(col)]); + res.covariances.push_back(p_cov); + } + col_start++; + } + + return res; +} + +CovarianceResult computeCovariance(ceres::Problem& problem, + const std::map>& param_masks, + const ceres::Covariance::Options& options) { std::vector blocks; problem.GetParameterBlocks(&blocks); const std::vector blocks_const(blocks.begin(), blocks.end()); - return computeCovariance(problem, blocks_const, options); + return computeCovariance(problem, blocks_const, param_masks, options); } CovarianceResult computeCovariance(ceres::Problem& problem, const std::vector& parameter_blocks, + const std::map>& param_masks, const ceres::Covariance::Options& options) { - std::vector> dummy_param_names; + std::map> dummy_param_names; for (std::size_t block_index = 0; block_index < parameter_blocks.size(); block_index++) { int size = problem.ParameterBlockSize(parameter_blocks[block_index]); @@ -60,14 +116,15 @@ CovarianceResult computeCovariance(ceres::Problem& problem, const std::vector>& parameter_names, + const std::map>& param_names, + const std::map>& param_masks, const ceres::Covariance::Options& options) { // Get all parameter blocks for the problem @@ -76,30 +133,48 @@ CovarianceResult computeCovariance(ceres::Problem& problem, const std::vector param_blocks_const(parameter_blocks.begin(), parameter_blocks.end()); - return computeCovariance(problem, param_blocks_const, parameter_names, options); + return computeCovariance(problem, param_blocks_const, param_names, param_masks, options); } CovarianceResult computeCovariance(ceres::Problem& problem, const std::vector& parameter_blocks, - const std::vector>& parameter_names, + const std::map>& param_names, + const std::map>& param_masks, const ceres::Covariance::Options& options) { // 0. Check user-specified arguments - if (parameter_blocks.size() != parameter_names.size()) + if (parameter_blocks.size() != param_names.size()) throw CovarianceException("Provided vector parameter_names is not same length as provided number of parameter " "blocks"); Eigen::Index n_params_in_selected = 0; - for (std::size_t index_block = 0; index_block < parameter_blocks.size(); index_block++) + std::vector tangent_space_indices; + std::vector tangent_space_labels; + for (const double* b : parameter_blocks) { - int block_size = problem.ParameterBlockSize(parameter_blocks[index_block]); - if (static_cast(block_size) != parameter_names[index_block].size()) + int block_size = problem.ParameterBlockSize(b); + if (static_cast(block_size) != param_names.at(b).size()) { std::stringstream ss; - ss << "Number of parameter labels provided for block " << index_block - << " does not match actual number of parameters in that block: " - << "have " << parameter_names[index_block].size() << " labels and " << block_size << " parameters"; + ss << "Number of parameter labels provided for block does not match actual number of parameters in that block: " + << "have " << param_names.at(b).size() << " labels and " << block_size << " parameters"; throw CovarianceException(ss.str()); } + + // Extract tangent space + std::vector masks; + auto it = param_masks.find(b); + if (it != param_masks.end()) masks = it->second; + + const std::vector& label = param_names.at(b); + for (std::size_t i = 0; i < static_cast(block_size); ++i) + { + if (std::find(masks.begin(), masks.end(), i) == masks.end()) + { + tangent_space_indices.push_back(n_params_in_selected + static_cast(i)); + tangent_space_labels.push_back(label.at(i)); + } + } + n_params_in_selected += block_size; } @@ -115,61 +190,18 @@ CovarianceResult computeCovariance(ceres::Problem& problem, const std::vector parameter_names_concatenated; - for (auto names : parameter_names) + // 2. Extract the tangent space cov matrix + Eigen::MatrixXd tangent_space_cov_matrix(tangent_space_labels.size(), tangent_space_labels.size()); + for (std::size_t r = 0; r < tangent_space_indices.size(); ++r) { - parameter_names_concatenated.insert(parameter_names_concatenated.end(), names.begin(), names.end()); - } - - // 5. Create NamedParams for covariance and correlation results, which include labels and values for the parameters. - // Uses top-right triangular part of matrix. - Eigen::Index col_start = 0; - for (Eigen::Index row = 0; row < correlation_matrix.rows(); row++) - { - for (Eigen::Index col = col_start; col < correlation_matrix.rows(); col++) + for (std::size_t c = 0; c < tangent_space_indices.size(); ++c) { - NamedParam p_corr; - p_corr.value = correlation_matrix(row, col); - - if (row == col) // diagonal element, standard deviation - { - p_corr.names = std::make_pair(parameter_names_concatenated[static_cast(row)], ""); - res.standard_deviations.push_back(p_corr); - continue; - } - - // otherwise off-diagonal element, correlation coefficient - p_corr.names = std::make_pair(parameter_names_concatenated[static_cast(row)], - parameter_names_concatenated[static_cast(col)]); - res.correlation_coeffs.push_back(p_corr); - - // for off-diagonal elements also get covariance - NamedParam p_cov; - p_cov.value = cov_matrix(row, col); - p_cov.names = std::make_pair(parameter_names_concatenated[static_cast(row)], - parameter_names_concatenated[static_cast(col)]); - res.covariances.push_back(p_cov); + tangent_space_cov_matrix(r, c) = cov_matrix(tangent_space_indices[r], tangent_space_indices[c]); } - col_start++; } - return res; + // 3. Get the covariance results + return computeCovarianceResults(tangent_space_cov_matrix, tangent_space_labels); } } // namespace industrial_calibration diff --git a/test/covariance_utest.cpp b/test/covariance_utest.cpp index ddcf1efa..e8703518 100644 --- a/test/covariance_utest.cpp +++ b/test/covariance_utest.cpp @@ -584,8 +584,14 @@ TEST(CovarianceAnalysis, CovarianceAnalysisFunctions) EXPECT_NEAR(0.0, result.y_center, 1e-5); EXPECT_NEAR(1.0, result.radius, 1e-5); - std::vector> labels_all({ { "circle_x" }, { "circle_y" }, { "circle_r" } }); - std::vector> labels_x_r({ { "circle_x" }, { "circle_r" } }); + std::map> labels_all; + labels_all[params_internal.data()] = { "circle_x" }; + labels_all[params_internal.data() + 1] = { "circle_y" }; + labels_all[params_internal.data() + 2] = { "circle_r" }; + + std::map> labels_x_r; + labels_x_r[params_internal.data()] = { "circle_x" }; + labels_x_r[params_internal.data() + 2] = { "circle_r" }; std::vector param_blocks_x_r(2); param_blocks_x_r[0] = params_internal.data(); @@ -653,15 +659,16 @@ TEST(CovarianceAnalysis, CovarianceAnalysisFunctions) // expect exception if number of labels for a parameter block is different from the number of parameters in that block // in the problem - std::vector> labels_x_r_wrong_size( - { { "circle_x", "circle_x_extra_entry" }, { "circle_r" } }); + std::map> labels_x_r_wrong_size; + labels_x_r_wrong_size[params_internal.data()] = { "circle_x", "circle_x_extra_entry" }; + labels_x_r_wrong_size[params_internal.data() + 2] = { "circle_r" }; EXPECT_THROW(computeCovariance(problem, param_blocks_x_r, labels_x_r_wrong_size), CovarianceException); // expect exception with empty parameter block vector EXPECT_THROW(computeCovariance(problem, std::vector(), labels_all), CovarianceException); // expect exception with empty labels vector - EXPECT_THROW(computeCovariance(problem, param_blocks_x_r, std::vector>()), + EXPECT_THROW(computeCovariance(problem, param_blocks_x_r, std::map>()), CovarianceException); } diff --git a/test/local_parameterization_utest.cpp b/test/local_parameterization_utest.cpp index f6cc086c..5939d63b 100644 --- a/test/local_parameterization_utest.cpp +++ b/test/local_parameterization_utest.cpp @@ -32,19 +32,12 @@ TEST(LocalParameterizationTests, SubsetParameterization) cost_block->SetNumResiduals(params.size()); problem.AddResidualBlock(cost_block, nullptr, params.data()); - // Wrong number of vectors - { - std::array, 3> mask; - EXPECT_THROW(addSubsetParameterization(problem, mask, { { params.data() } }), OptimizationException); - EXPECT_EQ(problem.GetParameterization(params.data()), nullptr); - } - // All values { - std::array, 1> mask; - mask.at(0).resize(params.size()); - std::iota(mask.at(0).begin(), mask.at(0).end(), 0); - EXPECT_NO_THROW(addSubsetParameterization(problem, mask, { { params.data() } })); + std::map> mask; + mask[params.data()] = std::vector(params.size()); + std::iota(mask[params.data()].begin(), mask[params.data()].end(), 0); + EXPECT_NO_THROW(addSubsetParameterization(problem, mask)); // Expect there to be no parameterization, but the entire block should be constant EXPECT_EQ(problem.GetParameterization(params.data()), nullptr); @@ -53,17 +46,18 @@ TEST(LocalParameterizationTests, SubsetParameterization) // Index out of range { - std::array, 1> mask; + std::map> mask; + mask[params.data()] = std::vector(params.size()); int bad_idx = params.size() * 2; - mask.at(0).insert(mask.at(0).begin(), { bad_idx, 0, 1, 2 }); - EXPECT_THROW(addSubsetParameterization(problem, mask, { { params.data() } }), OptimizationException); + mask[params.data()].insert(mask[params.data()].begin(), { bad_idx, 0, 1, 2 }); + EXPECT_THROW(addSubsetParameterization(problem, mask), OptimizationException); EXPECT_EQ(problem.GetParameterization(params.data()), nullptr); } // Empty mask { - std::array, 1> mask; - EXPECT_NO_THROW(addSubsetParameterization(problem, mask, { { params.data() } })); + std::map> mask; + EXPECT_NO_THROW(addSubsetParameterization(problem, mask)); // An empty mask should not have added any local parameterization EXPECT_EQ(problem.GetParameterization(params.data()), nullptr); } @@ -71,13 +65,14 @@ TEST(LocalParameterizationTests, SubsetParameterization) // Hold the zero-th row constant Eigen::ArrayXXd original_params = params; { - std::array, 1> mask; + std::map> mask; + mask[params.data()] = std::vector(); // Remember, Eigen stores values internally in column-wise order for (Eigen::Index i = 0; i < params.cols(); ++i) { - mask.at(0).push_back(i * params.rows()); + mask[params.data()].push_back(i * params.rows()); } - EXPECT_NO_THROW(addSubsetParameterization(problem, mask, { { params.data() } })); + EXPECT_NO_THROW(addSubsetParameterization(problem, mask)); EXPECT_NE(problem.GetParameterization(params.data()), nullptr); } diff --git a/test/src/circle_fit.cpp b/test/src/circle_fit.cpp index 41c7d345..b39c6952 100644 --- a/test/src/circle_fit.cpp +++ b/test/src/circle_fit.cpp @@ -40,13 +40,16 @@ CircleFitResult optimize(const CircleFitProblem& params) std::cout << summary.BriefReport() << std::endl; + std::map> param_block_labels; + param_block_labels[circle_params.data()] = params.labels; + result.converged = summary.termination_type == ceres::TerminationType::CONVERGENCE; result.x_center = circle_params[0]; result.y_center = circle_params[1]; result.radius = pow(circle_params[2], 2); result.initial_cost_per_obs = summary.initial_cost / summary.num_residuals; result.final_cost_per_obs = summary.final_cost / summary.num_residuals; - result.covariance = computeCovariance(problem, std::vector>({ params.labels })); + result.covariance = computeCovariance(problem, param_block_labels); return result; }