Function profiling and optimization

CMakeLists.txt

@@ -31,7 +31,7 @@ target_include_directories(${PROJECT_NAME}
 
 if(BUILD_TESTING)
   find_package(GTest REQUIRED)
-  find_package(mrpt-poses 2.10 REQUIRED)
+  find_package(mrpt-poses 2.4.2 REQUIRED)
 
   set(TEST_NAMES
   pose_composition_test

include/covariance_geometry/covariance_representation.hpp

@@ -67,13 +67,15 @@ void jacobianQuaternionNormalization(
   / @brief jacobian of the transformation from quaternion to RPY
   */
 
-void jacobianQuaternionToRPY(const Eigen::Quaterniond & quaternion, Eigen::Matrix3_4d & jacobian);
+void jacobianQuaternionToRPY(
+  const Eigen::Quaterniond & quaternion,
+  Eigen::Ref<Eigen::Matrix3_4d> jacobian);
 
 /*
   / @brief jacobian of the transformation from RPY to quaternion
   */
 
-void jacobianRPYToQuaternion(const Eigen::Vector3d & rpy, Eigen::Matrix4_3d & jacobian);
+void jacobianRPYToQuaternion(const Eigen::Vector3d & rpy, Eigen::Ref<Eigen::Matrix4_3d> jacobian);
 
 /*
   / @brief jacobian of the transformation from normalized quaternion to RPY

include/covariance_geometry/pose_covariance_composition.hpp

@@ -71,19 +71,22 @@ void JacobianPosePoseCompositionB(const PoseQuaternion & pose_a, Eigen::Matrix7d
   / @brief Compute pose-point composition function jacobian wrt pose
   */
 void JacobianPosePointComposition(
-  const PoseQuaternion & pose, const Eigen::Vector3d & point, Eigen::Matrix3_7d & jacobian);
+  const PoseQuaternion & pose, const Eigen::Vector3d & point,
+  Eigen::Ref<Eigen::Matrix3_7d> jacobian);
 
 /*
   / @brief Compute pose-point composition function jacobian wrt point
   */
-void JacobianPosePointComposition(const PoseQuaternion & pose, Eigen::Matrix3d & jacobian);
+void JacobianPosePointComposition(
+  const PoseQuaternion & pose,
+  Eigen::Ref<Eigen::Matrix3d> jacobian);
 
 /*
   / @brief Compute pose-point composition function jacobian wrt pose quaternion
   */
 void JacobianQuaternionPointComposition(
   const Eigen::Quaterniond & quaternion, const Eigen::Vector3d & point,
-  Eigen::Matrix3_4d & jacobian);
+  Eigen::Ref<Eigen::Matrix3_4d> jacobian);
 
 }  // namespace covariance_geometry
 

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/covariance_representation.cpp

@@ -13,7 +13,6 @@
 // limitations under the License.
 
 #include "covariance_geometry/covariance_representation.hpp"
-// #include "covariance_geometry/pose_representation.hpp"
 
 using PoseQuaternion = std::pair<Eigen::Vector3d, Eigen::Quaterniond>;
 using PoseRPY = std::pair<Eigen::Vector3d, Eigen::Vector3d>;
@@ -26,7 +25,7 @@ void covariance3DRPYTo3DQuaternion(
   // Equation 2.8 pag. 14 A tutorial on SE(3) transformation parameterizations and on-manifold optimization
   Eigen::Matrix7_6d jacobian = Eigen::Matrix7_6d::Zero();
   jacobian3DRPYTo3DQuaternion(rpy, jacobian);
-  covariance_quaternion = jacobian * covariance_rpy * jacobian.transpose();
+  covariance_quaternion.noalias() = jacobian * covariance_rpy * jacobian.transpose();
 }
 
 void covariance3DQuaternionTo3DRPY(
@@ -36,110 +35,111 @@ void covariance3DQuaternionTo3DRPY(
   // Equation 2.12 pag. 16 A tutorial on SE(3) transformation parameterizations and on-manifold optimization
   Eigen::Matrix6_7d jacobian = Eigen::Matrix6_7d::Zero();
   jacobian3DQuaternionTo3DRPY(quaternion, jacobian);
-  covariance_rpy = jacobian * covariance_quaternion * jacobian.transpose();
+  covariance_rpy.noalias() = jacobian * covariance_quaternion * jacobian.transpose();
 }
 
 void jacobian3DQuaternionTo3DRPY(
   const Eigen::Quaterniond & quaternion, Eigen::Matrix6_7d & jacobian)
 {
   // Equation 2.13 pag. 16 A tutorial on SE(3) transformation parameterizations and on-manifold optimization
-  Eigen::Matrix3_4d j34_tmp;
-  jacobian.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
-  jacobianQuaternionToRPY(quaternion, j34_tmp);
-  jacobian.block<3, 4>(3, 3) = j34_tmp;
+  jacobian.block<3, 3>(0, 0).diagonal() = Eigen::Vector3d::Ones();
+  jacobianQuaternionToRPY(quaternion, jacobian.block<3, 4>(3, 3));
 }
 
 void jacobian3DRPYTo3DQuaternion(const Eigen::Vector3d & rpy, Eigen::Matrix7_6d & jacobian)
 {
   // Equation 2.9a pag. 14 A tutorial on SE(3) transformation parameterizations and on-manifold optimization
-  Eigen::Matrix4_3d j43_tmp;
-  jacobian.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
-  jacobianRPYToQuaternion(rpy, j43_tmp);
-  jacobian.block<4, 3>(3, 3) = j43_tmp;
+  jacobian.block<3, 3>(0, 0).diagonal() = Eigen::Vector3d::Ones();
+  jacobianRPYToQuaternion(rpy, jacobian.block<4, 3>(3, 3));
 }
 
 void jacobianQuaternionNormalization(
   const Eigen::Quaterniond & quaternion, Eigen::Matrix4d & jacobian)
 {
   // Equation 1.7 pag. 11 A tutorial on SE(3) transformation parameterizations and on-manifold optimization
-  // Eigen::Quaterniond is quaternion in the form (x,y,z,w)
-  jacobian = Eigen::Matrix4d::Zero();
-  jacobian(0, 0) = quaternion.w() * quaternion.w() + quaternion.y() * quaternion.y() +
-    quaternion.z() * quaternion.z();
-  jacobian(1, 1) = quaternion.w() * quaternion.w() + quaternion.x() * quaternion.x() +
-    quaternion.z() * quaternion.z();
-  jacobian(2, 2) = quaternion.w() * quaternion.w() + quaternion.x() * quaternion.x() +
-    quaternion.y() * quaternion.y();
-  jacobian(3, 3) = quaternion.x() * quaternion.x() + quaternion.y() * quaternion.y() +
-    quaternion.z() * quaternion.z();
-  jacobian(0, 1) = -quaternion.x() * quaternion.y();
-  jacobian(0, 2) = -quaternion.x() * quaternion.z();
-  jacobian(0, 3) = -quaternion.x() * quaternion.w();
-
-  jacobian(1, 0) = -quaternion.x() * quaternion.y();
-  jacobian(1, 2) = -quaternion.y() * quaternion.z();
-  jacobian(1, 3) = -quaternion.y() * quaternion.w();
-
-  jacobian(2, 0) = -quaternion.x() * quaternion.z();
-  jacobian(2, 1) = -quaternion.y() * quaternion.z();
-  jacobian(2, 3) = -quaternion.z() * quaternion.w();
-
-  jacobian(3, 0) = -quaternion.x() * quaternion.w();
-  jacobian(3, 1) = -quaternion.y() * quaternion.w();
-  jacobian(3, 2) = -quaternion.z() * quaternion.w();
-
-  jacobian = jacobian / std::pow(quaternion.norm(), 3);
+  auto qx = quaternion.x();
+  auto qy = quaternion.y();
+  auto qz = quaternion.z();
+  auto qw = quaternion.w();
+
+  jacobian(0, 0) = qw * qw + qy * qy + qz * qz;
+  jacobian(1, 1) = qw * qw + qx * qx + qz * qz;
+  jacobian(2, 2) = qw * qw + qx * qx + qy * qy;
+  jacobian(3, 3) = qx * qx + qy * qy + qz * qz;
+
+  jacobian(0, 1) = -qx * qy;
+  jacobian(0, 2) = -qx * qz;
+  jacobian(0, 3) = -qx * qw;
+
+  jacobian(1, 0) = -qx * qy;
+  jacobian(1, 2) = -qy * qz;
+  jacobian(1, 3) = -qy * qw;
+
+  jacobian(2, 0) = -qx * qz;
+  jacobian(2, 1) = -qy * qz;
+  jacobian(2, 3) = -qz * qw;
+
+  jacobian(3, 0) = -qx * qw;
+  jacobian(3, 1) = -qy * qw;
+  jacobian(3, 2) = -qz * qw;
+
+  jacobian /= std::pow(quaternion.norm(), 3);
 }
 
-void jacobianRPYToQuaternion(const Eigen::Vector3d & rpy, Eigen::Matrix4_3d & jacobian)
+void jacobianRPYToQuaternion(const Eigen::Vector3d & rpy, Eigen::Ref<Eigen::Matrix4_3d> jacobian)
 {
   // Equation 2.9b pag. 14 A tutorial on SE(3) transformation parameterizations and on-manifold optimization
-  const double ccc = cos(rpy.x() / 2.0) * cos(rpy.y() / 2.0) * cos(rpy.z() / 2.0);
-  const double ccs = cos(rpy.x() / 2.0) * cos(rpy.y() / 2.0) * sin(rpy.z() / 2.0);
-  const double csc = cos(rpy.x() / 2.0) * sin(rpy.y() / 2.0) * cos(rpy.z() / 2.0);
-  const double css = cos(rpy.x() / 2.0) * sin(rpy.y() / 2.0) * sin(rpy.z() / 2.0);
-  const double scc = sin(rpy.x() / 2.0) * cos(rpy.y() / 2.0) * cos(rpy.z() / 2.0);
-  const double scs = sin(rpy.x() / 2.0) * cos(rpy.y() / 2.0) * sin(rpy.z() / 2.0);
-  const double ssc = sin(rpy.x() / 2.0) * sin(rpy.y() / 2.0) * cos(rpy.z() / 2.0);
-  const double sss = sin(rpy.x() / 2.0) * sin(rpy.y() / 2.0) * sin(rpy.z() / 2.0);
+  const double r2 = 0.5 * rpy.x();
+  const double p2 = 0.5 * rpy.y();
+  const double y2 = 0.5 * rpy.z();
+  const double ccc = cos(r2) * cos(p2) * cos(y2);
+  const double ccs = cos(r2) * cos(p2) * sin(y2);
+  const double csc = cos(r2) * sin(p2) * cos(y2);
+  const double css = cos(r2) * sin(p2) * sin(y2);
+  const double scc = sin(r2) * cos(p2) * cos(y2);
+  const double scs = sin(r2) * cos(p2) * sin(y2);
+  const double ssc = sin(r2) * sin(p2) * cos(y2);
+  const double sss = sin(r2) * sin(p2) * sin(y2);
 
   // dqx()/d(rpy)
-  jacobian(0, 0) = 0.5 * (ccc + sss);
-  jacobian(0, 1) = 0.5 * -(ssc + ccs);
-  jacobian(0, 2) = 0.5 * -(csc + scs);
+  jacobian(0, 0) = (ccc + sss);
+  jacobian(0, 1) = -(ssc + ccs);
+  jacobian(0, 2) = -(csc + scs);
 
   // dqy()/d(rpy)
-  jacobian(1, 0) = 0.5 * (ccs - ssc);
-  jacobian(1, 1) = 0.5 * (ccc - sss);
-  jacobian(1, 2) = 0.5 * (scc - css);
+  jacobian(1, 0) = (ccs - ssc);
+  jacobian(1, 1) = (ccc - sss);
+  jacobian(1, 2) = (scc - css);
 
   // dqz()/d(rpy)
-  jacobian(2, 0) = 0.5 * -(csc + scs);
-  jacobian(2, 1) = 0.5 * -(css + scc);
-  jacobian(2, 2) = 0.5 * (ccc + sss);
+  jacobian(2, 0) = -(csc + scs);
+  jacobian(2, 1) = -(css + scc);
+  jacobian(2, 2) = (ccc + sss);
 
   // dqw()/d(rpy)
-  jacobian(3, 0) = 0.5 * (css - scc);
-  jacobian(3, 1) = 0.5 * (scs - csc);
-  jacobian(3, 2) = 0.5 * (ssc - ccs);
+  jacobian(3, 0) = (css - scc);
+  jacobian(3, 1) = (scs - csc);
+  jacobian(3, 2) = (ssc - ccs);
 
-  jacobian = (1e-6 < jacobian.array().abs()).select(jacobian, 0.0);
+  jacobian *= 0.5;
 }
 
-void jacobianQuaternionToRPY(const Eigen::Quaterniond & quaternion, Eigen::Matrix3_4d & jacobian)
+void jacobianQuaternionToRPY(
+  const Eigen::Quaterniond & quaternion,
+  Eigen::Ref<Eigen::Matrix3_4d> jacobian)
 {
   // Equation 2.14 pag. 16 A tutorial on SE(3) transformation parameterizations and on-manifold optimization
   // d(rpy)()/d(quaternion) = d(rpy)()/d(quaternion_norm) * d(quaternion_norm)()/d(quaternion)
   // d(rpy)()/d(quaternion_norm) = jacobian_rpy_norm
-  Eigen::Matrix3_4d jacobian_rpy_norm = Eigen::Matrix3_4d::Zero();
+  Eigen::Matrix3_4d jacobian_rpy_norm;
   jacobianQuaternionNormalToRPY(quaternion.normalized(), jacobian_rpy_norm);
 
   // d(quaternion_norm)()/d(quaternion) = jacobian_norm
-  Eigen::Matrix4d jacobian_norm = Eigen::Matrix4d::Zero();
+  Eigen::Matrix4d jacobian_norm;
   jacobianQuaternionNormalization(quaternion, jacobian_norm);
 
   // d(rpy)()/d(quaternion) = d(rpy)()/d(quaternion_norm) * d(quaternion_norm)()/d(quaternion)
-  jacobian = jacobian_rpy_norm * jacobian_norm;
+  jacobian.noalias() = jacobian_rpy_norm * jacobian_norm;
 }
 
 void jacobianQuaternionNormalToRPY(
@@ -151,62 +151,46 @@ void jacobianQuaternionNormalToRPY(
   auto qw = quaternion.w();
   const auto discr = qw * qy - qx * qz;
 
-  jacobian = Eigen::Matrix3_4d::Zero();
   if (discr > 0.49999) {
     // pitch = 90 deg
-    jacobian(2, 0) = -2 / (qw * ((qx * qx / qw * qw) + 1));
-    jacobian(2, 3) = (2 * qx) / (qw * qw * ((qx * qx / qw * qw) + 1));
+    jacobian = Eigen::Matrix3_4d::Zero();
+    jacobian(2, 0) = -2.0 / (qw * ((qx * qx / qw * qw) + 1.0));
+    jacobian(2, 3) = (2.0 * qx) / (qw * qw * ((qx * qx / qw * qw) + 1.0));
     return;
   } else if (discr < -0.49999) {
     // pitch = -90 deg
-    jacobian(2, 0) = 2 / (qw * ((qx * qx / qw * qw) + 1));
-    jacobian(2, 3) = (-2 * qx) / (qw * qw * ((qx * qx / qw * qw) + 1));
+    jacobian = Eigen::Matrix3_4d::Zero();
+    jacobian(2, 0) = 2.0 / (qw * ((qx * qx / qw * qw) + 1.0));
+    jacobian(2, 3) = (-2.0 * qx) / (qw * qw * ((qx * qx / qw * qw) + 1.0));
     return;
   } else {
     // Non-degenerate case:
-    jacobian(0, 0) =
-      -((2 * qw) / (2 * qx * qx + 2 * qy * qy - 1) -
-      (4 * qx * (2 * qw * qx + 2 * qy * qz)) / std::pow((2 * qx * qx + 2 * qy * qy - 1), 2)) /
-      (std::pow((2 * qw * qx + 2 * qy * qz), 2) / std::pow((2 * qx * qx + 2 * qy * qy - 1), 2) + 1);
-    jacobian(0, 1) =
-      -((2 * qz) / (2 * qx * qx + 2 * qy * qy - 1) -
-      (4 * qy * (2 * qw * qx + 2 * qy * qz)) / std::pow((2 * qx * qx + 2 * qy * qy - 1), 2)) /
-      (std::pow((2 * qw * qx + 2 * qy * qz), 2) / std::pow((2 * qx * qx + 2 * qy * qy - 1), 2) + 1);
-    jacobian(0, 2) =
-      -(2 * qy) /
-      ((std::pow((2 * qw * qx + 2 * qy * qz), 2) / std::pow((2 * qx * qx + 2 * qy * qy - 1), 2) +
-      1) *
-      (2 * qx * qx + 2 * qy * qy - 1));
-    jacobian(0, 3) =
-      -(2 * qx) /
-      ((std::pow((2 * qw * qx + 2 * qy * qz), 2) / std::pow((2 * qx * qx + 2 * qy * qy - 1), 2) +
-      1) *
-      (2 * qx * qx + 2 * qy * qy - 1));
-
-    jacobian(1, 0) = -(2 * qz) / std::sqrt(1 - std::pow((2 * qw * qy - 2 * qx * qz), 2));
-    jacobian(1, 1) = (2 * qw) / std::sqrt(1 - std::pow((2 * qw * qy - 2 * qx * qz), 2));
-    jacobian(1, 2) = -(2 * qx) / std::sqrt(1 - std::pow((2 * qw * qy - 2 * qx * qz), 2));
-    jacobian(1, 3) = (2 * qy) / std::sqrt(1 - std::pow((2 * qw * qy - 2 * qx * qz), 2));
-
-    jacobian(2, 0) =
-      -(2 * qy) /
-      ((std::pow((2 * qw * qz + 2 * qx * qy), 2) / std::pow((2 * qy * qy + 2 * qz * qz - 1), 2) +
-      1) *
-      (2 * qy * qy + 2 * qz * qz - 1));
-    jacobian(2, 1) =
-      -((2 * qx) / (2 * qy * qy + 2 * qz * qz - 1) -
-      (4 * qy * (2 * qw * qz + 2 * qx * qy)) / std::pow((2 * qy * qy + 2 * qz * qz - 1), 2)) /
-      (std::pow((2 * qw * qz + 2 * qx * qy), 2) / std::pow((2 * qy * qy + 2 * qz * qz - 1), 2) + 1);
-    jacobian(2, 2) =
-      -((2 * qw) / (2 * qy * qy + 2 * qz * qz - 1) -
-      (4 * qz * (2 * qw * qz + 2 * qx * qy)) / std::pow((2 * qy * qy + 2 * qz * qz - 1), 2)) /
-      (std::pow((2 * qw * qz + 2 * qx * qy), 2) / std::pow((2 * qy * qy + 2 * qz * qz - 1), 2) + 1);
-    jacobian(2, 3) =
-      -(2 * qz) /
-      ((std::pow((2 * qw * qz + 2 * qx * qy), 2) / std::pow((2 * qy * qy + 2 * qz * qz - 1), 2) +
-      1) *
-      (2 * qy * qy + 2 * qz * qz - 1));
-
+    const double c0 = (2.0 * qx * qx + 2.0 * qy * qy - 1.0);
+    const double c1 = (2.0 * qw * qx + 2.0 * qy * qz);
+    const double c2 = (2.0 * qw * qz + 2.0 * qx * qy);
+    const double c3 = (2.0 * qy * qy + 2.0 * qz * qz - 1.0);
+    const double c0_2 = c0 * c0;
+    const double c1_2 = c1 * c1;
+    const double c2_2 = c2 * c2;
+    const double c3_2 = c3 * c3;
+    const double c4_i = 1.0 / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2));
+    const double c5 = (c1_2 / c0_2 + 1.0);
+    const double c6 = (c2_2 / c3_2 + 1.0);
+
+    jacobian(0, 0) = -(2.0 * qw / c0 - (4.0 * qx * c1) / c0_2) / c5;
+    jacobian(0, 1) = -(2.0 * qz / c0 - (4.0 * qy * c1) / c0_2) / c5;
+    jacobian(0, 2) = -(2.0 * qy) / (c5 * c0);
+    jacobian(0, 3) = -(2.0 * qx) / (c5 * c0);
+
+    jacobian(1, 0) = -(2.0 * qz) * c4_i;
+    jacobian(1, 1) = (2.0 * qw) * c4_i;
+    jacobian(1, 2) = -(2.0 * qx) * c4_i;
+    jacobian(1, 3) = (2.0 * qy) * c4_i;
+
+    jacobian(2, 0) = -(2.0 * qy) / (c6 * c3);
+    jacobian(2, 1) = -((2.0 * qx) / c3 - (4.0 * qy * c2) / c3_2) / c6;
+    jacobian(2, 2) = -((2.0 * qw) / c3 - (4.0 * qz * c2) / c3_2) / c6;
+    jacobian(2, 3) = -(2.0 * qz) / (c6 * c3);
     return;
   }
 }