Skip to content

Commit

Permalink
Merge pull request #10 from andreaostuni/profiling
Browse files Browse the repository at this point in the history 
Function profiling and optimization
  • Loading branch information
@andreaostuni
andreaostuni authored Jan 31, 2024
2 parents 2ea2425 + 152a263 commit 4f3b3a8
Show file tree
Hide file tree
Showing 9 changed files with 218 additions and 238 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
6 changes: 4 additions & 2 deletions include/covariance_geometry/covariance_representation.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
9 changes: 6 additions & 3 deletions include/covariance_geometry/pose_covariance_composition.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
10 changes: 5 additions & 5 deletions include/covariance_geometry/pose_covariance_representation.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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
Expand Down
208 changes: 96 additions & 112 deletions src/covariance_representation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>;
Expand All @@ -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(
Expand All @@ -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(
Expand All @@ -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;
}
}
Expand Down
Loading

0 comments on commit 4f3b3a8

Please sign in to comment.