pose covariance representation optimization and clean up

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

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<Eigen::Matrix<double, 3, 7>> 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<double, 3, 7> j_qi_top;
-  Eigen::Matrix<double, 3, 4> 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<double, 4> 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;
 }

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()}};