From 049cf1300414a399bd19b09ad99032a9b25f7571 Mon Sep 17 00:00:00 2001 From: mripperger Date: Tue, 30 Jun 2020 09:46:52 -0500 Subject: [PATCH] Updated to use angle axis representation rather than locally parameterized quaternion --- .../dh_chain_kinematic_calibration.h | 35 ++++++++++- .../dh_chain_kinematic_calibration.cpp | 63 ++++++++++--------- .../dh_chain_kinematic_calibration_utest.cpp | 28 +++++---- 3 files changed, 83 insertions(+), 43 deletions(-) diff --git a/rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h b/rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h index 8fd79708..cd9ce38d 100644 --- a/rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h +++ b/rct_optimizations/include/rct_optimizations/dh_chain_kinematic_calibration.h @@ -2,7 +2,6 @@ #include #include - #include namespace rct_optimizations @@ -66,8 +65,38 @@ class DualDHChainCost2D3D Isometry3 createTransform(T const *const *params, const std::size_t idx) const { Eigen::Map> t(params[idx]); - Eigen::Map> q(params[idx + 1]); - return Eigen::Translation(t) * q; + Eigen::Map> aa(params[idx + 1]); + + Isometry3 result = Isometry3::Identity() * Eigen::Translation(t); + + T aa_norm = aa.norm(); + if (aa_norm > std::numeric_limits::epsilon()) + { + result *= Eigen::AngleAxis(aa_norm, aa.normalized()); + } + + return result; + } + + static std::vector constructParameters(Eigen::MatrixX4d &camera_chain_dh_offsets, + Eigen::MatrixX4d &target_chain_dh_offsets, + Eigen::Vector3d &t_cm_to_c, + Eigen::Vector3d &aa_cm_to_c, + Eigen::Vector3d &t_tm_to_t, + Eigen::Vector3d &aa_tm_to_t, + Eigen::Vector3d &t_ccb_to_tcb, + Eigen::Vector3d &aa_ccb_to_tcb) + { + std::vector parameters; + parameters.push_back(camera_chain_dh_offsets.data()); + parameters.push_back(target_chain_dh_offsets.data()); + parameters.push_back(t_cm_to_c.data()); + parameters.push_back(aa_cm_to_c.data()); + parameters.push_back(t_tm_to_t.data()); + parameters.push_back(aa_tm_to_t.data()); + parameters.push_back(t_ccb_to_tcb.data()); + parameters.push_back(aa_ccb_to_tcb.data()); + return parameters; } template diff --git a/rct_optimizations/src/rct_optimizations/dh_chain_kinematic_calibration.cpp b/rct_optimizations/src/rct_optimizations/dh_chain_kinematic_calibration.cpp index 4c1f4b90..9d9012bc 100644 --- a/rct_optimizations/src/rct_optimizations/dh_chain_kinematic_calibration.cpp +++ b/rct_optimizations/src/rct_optimizations/dh_chain_kinematic_calibration.cpp @@ -8,20 +8,36 @@ namespace rct_optimizations { +Eigen::Isometry3d createTransform(const Eigen::Vector3d& t, const Eigen::Vector3d& aa) +{ + Eigen::Isometry3d result = Eigen::Isometry3d::Identity() * Eigen::Translation3d(t); + + double aa_norm = aa.norm(); + if (aa_norm > std::numeric_limits::epsilon()) + { + result *= Eigen::AngleAxisd(aa_norm, aa.normalized()); + } + + return result; +} + KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D ¶ms) { // Initialize the optimization variables // Camera mount to camera (cm_to_c) quaternion and translation - Eigen::Quaterniond q_cm_to_c(params.camera_mount_to_camera_guess.rotation()); 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()); // Target mount to target (cm_to_c) quaternion and translation Eigen::Vector3d t_tm_to_t(params.target_mount_to_target_guess.translation()); - Eigen::Quaterniond q_tm_to_t(params.target_mount_to_target_guess.rotation()); + Eigen::AngleAxisd rot_tm_to_t(params.target_mount_to_target_guess.rotation()); + Eigen::Vector3d aa_tm_to_t(rot_tm_to_t.angle() * rot_tm_to_t.axis()); // Camera chain base to target_chain_base (ccb_to_tcb) quaternion and translation Eigen::Vector3d t_ccb_to_tcb(params.camera_base_to_target_base_guess.translation()); - Eigen::Quaterniond q_ccb_to_tcb(params.camera_base_to_target_base_guess.rotation()); + Eigen::AngleAxisd rot_ccb_to_tcb(params.camera_base_to_target_base_guess.rotation()); + Eigen::Vector3d aa_ccb_to_tcb(rot_ccb_to_tcb.angle() * rot_ccb_to_tcb.axis()); // Create containers for the kinematic chain DH offsets // Ceres will not work with parameter blocks of size zero, so create a dummy set of DH offsets for chains with DoF == 0 @@ -38,15 +54,15 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D ¶m target_chain_dh_offsets = Eigen::MatrixX4d::Zero(1, 4); // Create a vector of the pointers to the optimization variables in the order that the cost function expects them - std::vector parameters; - parameters.push_back(camera_chain_dh_offsets.data()); - parameters.push_back(target_chain_dh_offsets.data()); - parameters.push_back(t_cm_to_c.data()); - parameters.push_back(q_cm_to_c.coeffs().data()); - parameters.push_back(t_tm_to_t.data()); - parameters.push_back(q_tm_to_t.coeffs().data()); - parameters.push_back(t_ccb_to_tcb.data()); - parameters.push_back(q_ccb_to_tcb.coeffs().data()); + std::vector parameters + = DualDHChainCost2D3D::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); // Set up the problem ceres::Problem problem; @@ -65,7 +81,7 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D ¶m observation.camera_chain_joints, observation.target_chain_joints); - auto *cost_block = new ceres::DynamicAutoDiffCostFunction(cost_fn); + auto *cost_block = new ceres::DynamicAutoDiffCostFunction(cost_fn); // Add the optimization parameters // DH parameters for both kinematic chains @@ -74,15 +90,15 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D ¶m // Camera mount to camera transform cost_block->AddParameterBlock(3); - cost_block->AddParameterBlock(4); + cost_block->AddParameterBlock(3); // Target mount to target transform cost_block->AddParameterBlock(3); - cost_block->AddParameterBlock(4); + cost_block->AddParameterBlock(3); // Camera kinematic chain to target kinematic chain transform cost_block->AddParameterBlock(3); - cost_block->AddParameterBlock(4); + cost_block->AddParameterBlock(3); // Residual error cost_block->SetNumResiduals(2); @@ -92,15 +108,6 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D ¶m } } - // Set the local parameterization for the quaternions - // We want to represent the quaternions with the minimum number of variables (3 vs. 4) in the optimization - problem.SetParameterization(q_cm_to_c.coeffs().data(), - new ceres::EigenQuaternionParameterization()); - problem.SetParameterization(q_tm_to_t.coeffs().data(), - new ceres::EigenQuaternionParameterization()); - problem.SetParameterization(q_ccb_to_tcb.coeffs().data(), - new ceres::EigenQuaternionParameterization()); - // Tell the optimization to keep constant the dummy DH offsets that might have been added to the 0-DoF chains if (params.camera_chain.dof() == 0) problem.SetParameterBlockConstant(camera_chain_dh_offsets.data()); @@ -130,9 +137,9 @@ KinematicCalibrationResult optimize(const KinematicCalibrationProblem2D3D ¶m result.final_cost_per_obs = summary.final_cost / summary.num_residuals; // Save the transforms - result.camera_mount_to_camera = Eigen::Translation3d(t_cm_to_c) * q_cm_to_c; - result.target_mount_to_target = Eigen::Translation3d(t_tm_to_t) * q_tm_to_t; - result.camera_base_to_target_base = Eigen::Translation3d(t_ccb_to_tcb) * q_ccb_to_tcb; + result.camera_mount_to_camera = createTransform(t_cm_to_c, aa_cm_to_c); + result.target_mount_to_target = createTransform(t_tm_to_t, aa_tm_to_t); + result.camera_base_to_target_base = createTransform(t_ccb_to_tcb, aa_ccb_to_tcb); // Save the DH parameter offsets if (params.camera_chain.dof() == 0) diff --git a/rct_optimizations/test/dh_chain_kinematic_calibration_utest.cpp b/rct_optimizations/test/dh_chain_kinematic_calibration_utest.cpp index dad45b14..96231427 100644 --- a/rct_optimizations/test/dh_chain_kinematic_calibration_utest.cpp +++ b/rct_optimizations/test/dh_chain_kinematic_calibration_utest.cpp @@ -43,15 +43,18 @@ TEST_F(DHChainKinematicCalibration, TestCostFunction) // Initialize the optimization variables // Camera mount to camera (cm_to_c) quaternion and translation Eigen::Vector3d t_cm_to_c(camera_mount_to_camera.translation()); - Eigen::Quaterniond q_cm_to_c(camera_mount_to_camera.rotation()); + Eigen::AngleAxisd rot_cm_to_c(camera_mount_to_camera.rotation()); + Eigen::Vector3d aa_cm_to_c(rot_cm_to_c.angle() * rot_cm_to_c.axis()); // Target mount to target (cm_to_c) quaternion and translation Eigen::Vector3d t_tm_to_t(target_mount_to_target.translation()); - Eigen::Quaterniond q_tm_to_t(target_mount_to_target.rotation()); + Eigen::AngleAxisd rot_tm_to_t(target_mount_to_target.rotation()); + Eigen::Vector3d aa_tm_to_t(rot_tm_to_t.angle() * rot_tm_to_t.axis()); // Camera chain base to target_chain_base (ccb_to_tcb) quaternion and translation Eigen::Vector3d t_ccb_to_tcb(camera_base_to_target_base.translation()); - Eigen::Quaterniond q_ccb_to_tcb(camera_base_to_target_base.rotation()); + Eigen::AngleAxisd rot_ccb_to_tcb(camera_base_to_target_base.rotation()); + Eigen::Vector3d aa_ccb_to_tcb(rot_ccb_to_tcb.angle() * rot_ccb_to_tcb.axis()); // Create containers for the kinematic chain DH offsets // Ceres will not work with parameter blocks of size zero, so create a dummy set of DH offsets for chains with DoF == 0 @@ -59,15 +62,15 @@ TEST_F(DHChainKinematicCalibration, TestCostFunction) Eigen::MatrixX4d target_chain_dh_offsets = Eigen::MatrixX4d::Zero(target_chain.dof(), 4); // Create a vector of the pointers to the optimization variables in the order that the cost function expects them - std::vector parameters; - parameters.push_back(camera_chain_dh_offsets.data()); - parameters.push_back(target_chain_dh_offsets.data()); - parameters.push_back(t_cm_to_c.data()); - parameters.push_back(q_cm_to_c.coeffs().data()); - parameters.push_back(t_tm_to_t.data()); - parameters.push_back(q_tm_to_t.coeffs().data()); - parameters.push_back(t_ccb_to_tcb.data()); - parameters.push_back(q_ccb_to_tcb.coeffs().data()); + std::vector parameters + = DualDHChainCost2D3D::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); // Create observations KinObservation2D3D::Set observations = test::createKinematicObservations(camera_chain, @@ -127,6 +130,7 @@ TEST_F(DHChainKinematicCalibration, TestCalibrationPerfectGuessPerfectDH) EXPECT_TRUE(result.converged); EXPECT_LT(result.initial_cost_per_obs, 1.0e-15); EXPECT_LT(result.final_cost_per_obs, 1.0e-15); + EXPECT_TRUE(result.camera_mount_to_camera.isApprox(camera_mount_to_camera)); EXPECT_TRUE(result.target_mount_to_target.isApprox(target_mount_to_target)); EXPECT_TRUE(result.camera_base_to_target_base.isApprox(camera_base_to_target_base));