diff --git a/include/covariance_geometry/pose_covariance_representation.hpp b/include/covariance_geometry/pose_covariance_representation.hpp index acb0ab8..e047d10 100644 --- a/include/covariance_geometry/pose_covariance_representation.hpp +++ b/include/covariance_geometry/pose_covariance_representation.hpp @@ -84,7 +84,7 @@ PoseRPY InversePose(const PoseRPY & pose_in); / @brief Inverse of a covariance matrix in quaternion representation */ -Eigen::Matrix7d inverseCovarianceQuaternion( +Eigen::Matrix7d InverseCovarianceQuaternion( const Eigen::Matrix7d & covariance_quaternion, const PoseQuaternion & pose); @@ -93,23 +93,23 @@ Eigen::Matrix7d inverseCovarianceQuaternion( This accepts pose in quaternion representation, since it is assumed to be used together with inversePose */ -Eigen::Matrix6d inverseCovarianceRPY(const Eigen::Matrix6d & covariance_rpy, const PoseRPY & pose); +Eigen::Matrix6d InverseCovarianceRPY(const Eigen::Matrix6d & covariance_rpy, const PoseRPY & pose); /* / @brief Inverse of a pose with covariance in quaternion representation */ -PoseQuaternionCovariance inversePose3DQuaternionCovarianceQuaternion( +PoseQuaternionCovariance InversePose3DQuaternionCovarianceQuaternion( const PoseQuaternionCovariance & pose_quaternion_covariance); /* / @brief Inverse of a pose with covariance in RPY representation */ -PoseRPYCovariance inversePose3DRPYCovarianceRPY(const PoseRPYCovariance & pose_rpy_covariance); +PoseRPYCovariance InversePose3DRPYCovarianceRPY(const PoseRPYCovariance & pose_rpy_covariance); /* / @brief Inverse of a pose in quaternion with covariance in RPY representation (ROS convention) */ -PoseQuaternionCovarianceRPY inversePose3DQuaternionCovarianceRPY( +PoseQuaternionCovarianceRPY InversePose3DQuaternionCovarianceRPY( const PoseQuaternionCovarianceRPY & pose_quaternion_covariance_rpy); } // namespace covariance_geometry diff --git a/src/pose_covariance_representation.cpp b/src/pose_covariance_representation.cpp index f671b01..94accbf 100644 --- a/src/pose_covariance_representation.cpp +++ b/src/pose_covariance_representation.cpp @@ -93,7 +93,7 @@ PoseQuaternion InversePose(const PoseQuaternion & pose_in) { PoseQuaternion pose_out; // Inverse of quaternion - pose_out.second = pose_in.second.inverse(); + pose_out.second = pose_in.second.conjugate(); // Inverse of translation pose_out.first = pose_out.second * -pose_in.first; return pose_out; @@ -110,13 +110,15 @@ PoseRPY InversePose(const PoseRPY & pose_in) return pose_out; } -Eigen::Matrix7d inverseCovarianceQuaternion( +Eigen::Matrix7d InverseCovarianceQuaternion( const Eigen::Matrix7d & covariance_quaternion, const PoseQuaternion & pose) { // Equation 6.3 pag. 34 A tutorial on SE(3) transformation parameterizations and on-manifold optimization Eigen::Matrix7d cov_inv; Eigen::Matrix7d j_qi = Eigen::Matrix7d::Zero(); + Eigen::Ref> j_qi_top = j_qi.topRows(3); + Eigen::Matrix4d j44; // Equation 4.4 pag. 27 A tutorial on SE(3) transformation parameterizations and on-manifold optimization // j_qi top 3x7 block @@ -128,10 +130,6 @@ Eigen::Matrix7d inverseCovarianceQuaternion( double qz = pose.second.z(); double qw = pose.second.w(); - Eigen::Matrix j_qi_top; - Eigen::Matrix j_qi_top_right; - Eigen::Matrix4d j44; - j_qi_top(0, 0) = 2.0 * (qy * qy + qz * qz) - 1.0; j_qi_top(0, 1) = -2.0 * (qw * qz + qx * qy); j_qi_top(0, 2) = 2.0 * (qw * qy - qx * qz); @@ -166,51 +164,54 @@ Eigen::Matrix7d inverseCovarianceQuaternion( // j_qi bottom right 4x4 block Eigen::DiagonalMatrix D(-1.0, -1.0, -1.0, 1.0); j_qi.block<4, 4>(3, 3) += D * j44; + + // j_qi bottom left 4x3 block + j_qi.block<4, 3>(3, 0).setZero(); + return j_qi * covariance_quaternion * j_qi.transpose(); } -Eigen::Matrix6d inverseCovarianceRPY(const Eigen::Matrix6d & covariance_rpy, const PoseRPY & pose) +Eigen::Matrix6d InverseCovarianceRPY(const Eigen::Matrix6d & covariance_rpy, const PoseRPY & pose) { // Easier to convert covariance to quaternion, invert, and then convert back to RPY PoseQuaternion pose_quaternion; Eigen::Matrix7d covariance_quaternion; + Eigen::Matrix6d covariance_rpy_out; + // Convert pose and covariance from RPY to quaternion Pose3DRPYTo3DQuaternion(pose, pose_quaternion); covariance3DRPYTo3DQuaternion(pose.second, covariance_rpy, covariance_quaternion); - // Invert pose and covariance - covariance_quaternion = inverseCovarianceQuaternion(covariance_quaternion, pose_quaternion); - // TODO: pose inversion is redundant, it is already done in the inverseCovarianceQuaternion - pose_quaternion = InversePose(pose_quaternion); - // Convert back to RPY - Eigen::Matrix6d covariance_rpy_out; - covariance3DQuaternionTo3DRPY(pose_quaternion.second, covariance_quaternion, covariance_rpy_out); + + // Invert covariance and covert it back to RPY + covariance_quaternion = InverseCovarianceQuaternion(covariance_quaternion, pose_quaternion); + covariance3DQuaternionTo3DRPY(pose_quaternion.second.conjugate(), covariance_quaternion, covariance_rpy_out); return covariance_rpy_out; } -PoseQuaternionCovariance inversePose3DQuaternionCovarianceQuaternion( +PoseQuaternionCovariance InversePose3DQuaternionCovarianceQuaternion( const PoseQuaternionCovariance & pose_quaternion_covariance) { PoseQuaternionCovariance pose_quaternion_covariance_out; // Invert pose pose_quaternion_covariance_out.first = InversePose(pose_quaternion_covariance.first); // Invert covariance - pose_quaternion_covariance_out.second = inverseCovarianceQuaternion( + pose_quaternion_covariance_out.second = InverseCovarianceQuaternion( pose_quaternion_covariance.second, pose_quaternion_covariance.first); return pose_quaternion_covariance_out; } -PoseRPYCovariance inversePose3DRPYCovarianceRPY(const PoseRPYCovariance & pose_rpy_covariance) +PoseRPYCovariance InversePose3DRPYCovarianceRPY(const PoseRPYCovariance & pose_rpy_covariance) { PoseRPYCovariance pose_rpy_covariance_out; // Invert pose pose_rpy_covariance_out.first = InversePose(pose_rpy_covariance.first); // Invert covariance - pose_rpy_covariance_out.second = inverseCovarianceRPY( + pose_rpy_covariance_out.second = InverseCovarianceRPY( pose_rpy_covariance.second, pose_rpy_covariance.first); return pose_rpy_covariance_out; } -PoseQuaternionCovarianceRPY inversePose3DQuaternionCovarianceRPY( +PoseQuaternionCovarianceRPY InversePose3DQuaternionCovarianceRPY( const PoseQuaternionCovarianceRPY & pose_quaternion_covariance_rpy) { PoseQuaternionCovarianceRPY pose_quaternion_covariance_rpy_out; @@ -220,7 +221,7 @@ PoseQuaternionCovarianceRPY inversePose3DQuaternionCovarianceRPY( // First convert pose in rpy, needed for the inverse of the covariance PoseRPY pose_rpy; Pose3DQuaternionTo3DRPY(pose_quaternion_covariance_rpy.first, pose_rpy); - pose_quaternion_covariance_rpy_out.second = inverseCovarianceRPY( + pose_quaternion_covariance_rpy_out.second = InverseCovarianceRPY( pose_quaternion_covariance_rpy.second, pose_rpy); return pose_quaternion_covariance_rpy_out; } diff --git a/test/pose_covariance_representation_test.cpp b/test/pose_covariance_representation_test.cpp index e709ee8..8ec26c6 100644 --- a/test/pose_covariance_representation_test.cpp +++ b/test/pose_covariance_representation_test.cpp @@ -258,7 +258,7 @@ TEST(PoseCovarianceInversion, InvertPoseQuaternionCovariance) pq.first.first = coord; pq.first.second = quat; pq.second = cov_eigen; - pq_inv = inversePose3DQuaternionCovarianceQuaternion(pq); + pq_inv = InversePose3DQuaternionCovarianceQuaternion(pq); CPose3DQuatPDFGaussian mrpt_pose_q, mrpt_pose_q_inv; mrpt_pose_q.mean = @@ -281,7 +281,7 @@ TEST(PoseCovarianceInversion, InvertPoseRPYCovariance) pr.first.first = coord; pr.first.second = rpy; pr.second = cov_eigen; - pr_inv = inversePose3DRPYCovarianceRPY(pr); + pr_inv = InversePose3DRPYCovarianceRPY(pr); std::cout << "pr_inv: \n" << pr_inv.first.first << std::endl; CPose3DPDFGaussian mrpt_pose_r, mrpt_pose_r_inv; @@ -304,7 +304,7 @@ TEST(PoseCovarianceInversion, InvertPoseQuaternionCovarianceRPY) pq.first.first = coord; pq.first.second = quat; pq.second = cov_eigen; - pq_inv = inversePose3DQuaternionCovarianceRPY(pq); + pq_inv = InversePose3DQuaternionCovarianceRPY(pq); CPose3DQuat mrpt_pose_q = {coord.x(), coord.y(), coord.z(), {quat.w(), quat.x(), quat.y(), quat.z()}};