From fe7e78ccf695102e23045a4fd142e95b7b7b5079 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Wed, 1 Mar 2023 12:56:47 -0600 Subject: [PATCH] Performance improvements found using callgrind (#852) --- .../include/tesseract_common/yaml_utils.h | 2 +- tesseract_common/src/utils.cpp | 4 +- .../include/tesseract_geometry/mesh_parser.h | 4 ++ .../include/tesseract_kinematics/core/utils.h | 9 ++-- tesseract_scene_graph/src/kdl_parser.cpp | 43 ++++++++++++++----- .../src/group_tool_center_points.cpp | 2 +- .../src/kdl_state_solver.cpp | 4 +- .../src/ofkt_state_solver.cpp | 2 +- tesseract_urdf/src/origin.cpp | 2 +- 9 files changed, 49 insertions(+), 23 deletions(-) diff --git a/tesseract_common/include/tesseract_common/yaml_utils.h b/tesseract_common/include/tesseract_common/yaml_utils.h index a7e3a7e0b77..6f10c179a00 100644 --- a/tesseract_common/include/tesseract_common/yaml_utils.h +++ b/tesseract_common/include/tesseract_common/yaml_utils.h @@ -295,7 +295,7 @@ struct convert Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY()); Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ()); - Eigen::Quaterniond rpy = yawAngle * pitchAngle * rollAngle; + Eigen::Quaterniond rpy{ yawAngle * pitchAngle * rollAngle }; out.linear() = rpy.toRotationMatrix(); } diff --git a/tesseract_common/src/utils.cpp b/tesseract_common/src/utils.cpp index 77b23b5f2a5..0ee9ccb789e 100644 --- a/tesseract_common/src/utils.cpp +++ b/tesseract_common/src/utils.cpp @@ -133,7 +133,7 @@ Eigen::Vector3d calcRotationalError(const Eigen::Ref& R) // Make sure that the angle is on [-pi, pi] const static double two_pi = 2.0 * M_PI; double angle = s * r12.angle(); - Eigen::Vector3d axis = s * r12.axis(); + Eigen::Vector3d axis{ s * r12.axis() }; angle = copysign(fmod(fabs(angle), two_pi), angle); if (angle < -M_PI) angle += two_pi; @@ -157,7 +157,7 @@ Eigen::Vector3d calcRotationalError2(const Eigen::Ref& R) // Make sure that the angle is on [0, 2 * pi] const static double two_pi = 2.0 * M_PI; double angle = s * r12.angle(); - Eigen::Vector3d axis = s * r12.axis(); + Eigen::Vector3d axis{ s * r12.axis() }; angle = copysign(fmod(fabs(angle), two_pi), angle); if (angle < 0) angle += two_pi; diff --git a/tesseract_geometry/include/tesseract_geometry/mesh_parser.h b/tesseract_geometry/include/tesseract_geometry/mesh_parser.h index 519b7085e1a..5ea49f13930 100644 --- a/tesseract_geometry/include/tesseract_geometry/mesh_parser.h +++ b/tesseract_geometry/include/tesseract_geometry/mesh_parser.h @@ -83,6 +83,7 @@ std::vector> extractMeshData(const aiScene* scene, bool material_and_texture) { std::vector> meshes; + meshes.reserve(node->mNumMeshes); aiMatrix4x4 transform = parent_transform; transform *= node->mTransformation; @@ -96,6 +97,7 @@ std::vector> extractMeshData(const aiScene* scene, std::shared_ptr> textures = nullptr; const aiMesh* a = scene->mMeshes[node->mMeshes[j]]; + vertices->reserve(a->mNumVertices); for (unsigned int i = 0; i < a->mNumVertices; ++i) { aiVector3D v = transform * a->mVertices[i]; @@ -129,6 +131,7 @@ std::vector> extractMeshData(const aiScene* scene, if (normals && a->HasNormals()) { vertex_normals = std::make_shared(); + vertex_normals->reserve(a->mNumVertices); for (unsigned int i = 0; i < a->mNumVertices; ++i) { aiVector3D v = transform * a->mNormals[i]; @@ -141,6 +144,7 @@ std::vector> extractMeshData(const aiScene* scene, if (vertex_colors && a->HasVertexColors(0)) { vertex_colors = std::make_shared(); + vertex_colors->reserve(a->mNumVertices); for (unsigned int i = 0; i < a->mNumVertices; ++i) { aiColor4D v = a->mColors[0][i]; diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/utils.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/utils.h index 376ba3561e3..3f06216bf78 100644 --- a/tesseract_kinematics/core/include/tesseract_kinematics/core/utils.h +++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/utils.h @@ -62,8 +62,7 @@ inline static void numericalJacobian(Eigen::Ref jacobian, Eigen::VectorXd njvals; double delta = 1e-8; tesseract_common::TransformMap poses = kin.calcFwdKin(joint_values); - Eigen::Isometry3d pose = poses[link_name]; - pose = change_base * pose; + Eigen::Isometry3d pose{ change_base * poses[link_name] }; for (int i = 0; i < static_cast(joint_values.size()); ++i) { @@ -72,13 +71,13 @@ inline static void numericalJacobian(Eigen::Ref jacobian, Eigen::Isometry3d updated_pose = kin.calcFwdKin(njvals)[link_name]; updated_pose = change_base * updated_pose; - Eigen::Vector3d temp = pose * link_point; - Eigen::Vector3d temp2 = updated_pose * link_point; + Eigen::Vector3d temp{ pose * link_point }; + Eigen::Vector3d temp2{ updated_pose * link_point }; jacobian(0, i) = (temp2.x() - temp.x()) / delta; jacobian(1, i) = (temp2.y() - temp.y()) / delta; jacobian(2, i) = (temp2.z() - temp.z()) / delta; - Eigen::VectorXd omega = (pose.rotation() * tesseract_common::calcRotationalError(pose.rotation().transpose() * + Eigen::Vector3d omega = (pose.rotation() * tesseract_common::calcRotationalError(pose.rotation().transpose() * updated_pose.rotation())) / delta; jacobian(3, i) = omega(0); diff --git a/tesseract_scene_graph/src/kdl_parser.cpp b/tesseract_scene_graph/src/kdl_parser.cpp index f42ce8e9bd6..328b6b4daa5 100644 --- a/tesseract_scene_graph/src/kdl_parser.cpp +++ b/tesseract_scene_graph/src/kdl_parser.cpp @@ -41,28 +41,51 @@ namespace tesseract_scene_graph KDL::Frame convert(const Eigen::Isometry3d& transform) { KDL::Frame frame; - frame.Identity(); - for (int i = 0; i < 3; ++i) - frame.p[i] = transform(i, 3); + // translation + frame.p[0] = transform(0, 3); + frame.p[1] = transform(1, 3); + frame.p[2] = transform(2, 3); - for (int i = 0; i < 9; ++i) - frame.M.data[i] = transform(i / 3, i % 3); + // rotation matrix + frame.M.data[0] = transform(0, 0); + frame.M.data[1] = transform(0, 1); + frame.M.data[2] = transform(0, 2); + frame.M.data[3] = transform(1, 0); + frame.M.data[4] = transform(1, 1); + frame.M.data[5] = transform(1, 2); + frame.M.data[6] = transform(2, 0); + frame.M.data[7] = transform(2, 1); + frame.M.data[8] = transform(2, 2); return frame; } Eigen::Isometry3d convert(const KDL::Frame& frame) { - Eigen::Isometry3d transform{ Eigen::Isometry3d::Identity() }; + Eigen::Isometry3d transform; // translation - for (int i = 0; i < 3; ++i) - transform(i, 3) = frame.p[i]; + transform(0, 3) = frame.p[0]; + transform(1, 3) = frame.p[1]; + transform(2, 3) = frame.p[2]; // rotation matrix - for (int i = 0; i < 9; ++i) - transform(i / 3, i % 3) = frame.M.data[i]; + transform(0, 0) = frame.M.data[0]; + transform(0, 1) = frame.M.data[1]; + transform(0, 2) = frame.M.data[2]; + transform(1, 0) = frame.M.data[3]; + transform(1, 1) = frame.M.data[4]; + transform(1, 2) = frame.M.data[5]; + transform(2, 0) = frame.M.data[6]; + transform(2, 1) = frame.M.data[7]; + transform(2, 2) = frame.M.data[8]; + + // Bottom row + transform(3, 0) = 0; + transform(3, 1) = 0; + transform(3, 2) = 0; + transform(3, 3) = 1; return transform; } diff --git a/tesseract_srdf/src/group_tool_center_points.cpp b/tesseract_srdf/src/group_tool_center_points.cpp index e0255ed50f1..7e73e4a93e0 100644 --- a/tesseract_srdf/src/group_tool_center_points.cpp +++ b/tesseract_srdf/src/group_tool_center_points.cpp @@ -139,7 +139,7 @@ GroupTCPs parseGroupTCPs(const tesseract_scene_graph::SceneGraph& /*scene_graph* Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY()); Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ()); - Eigen::Quaterniond rpy = yawAngle * pitchAngle * rollAngle; + Eigen::Quaterniond rpy{ yawAngle * pitchAngle * rollAngle }; tcp.linear() = rpy.toRotationMatrix(); } diff --git a/tesseract_state_solver/src/kdl_state_solver.cpp b/tesseract_state_solver/src/kdl_state_solver.cpp index e18de662c2c..95b1c23f570 100644 --- a/tesseract_state_solver/src/kdl_state_solver.cpp +++ b/tesseract_state_solver/src/kdl_state_solver.cpp @@ -217,7 +217,7 @@ bool KDLStateSolver::isActiveLinkName(const std::string& link_name) const { return (std::find(data_.active_link_names.begin(), data_.active_link_names.end(), link_name) != data_.active_link_names.end()); -}; +} bool KDLStateSolver::hasLinkName(const std::string& link_name) const { @@ -316,7 +316,7 @@ void KDLStateSolver::calculateTransformsHelper(SceneState& state, current_frame = GetTreeElementSegment(current_element).pose(0); Eigen::Isometry3d local_frame = convert(current_frame); - Eigen::Isometry3d global_frame = parent_frame * local_frame; + Eigen::Isometry3d global_frame{ parent_frame * local_frame }; state.link_transforms[current_element.segment.getName()] = global_frame; if (current_element.segment.getName() != data_.tree.getRootSegment()->first) state.joint_transforms[current_element.segment.getJoint().getName()] = global_frame; diff --git a/tesseract_state_solver/src/ofkt_state_solver.cpp b/tesseract_state_solver/src/ofkt_state_solver.cpp index 6755d122e13..4f28d457708 100644 --- a/tesseract_state_solver/src/ofkt_state_solver.cpp +++ b/tesseract_state_solver/src/ofkt_state_solver.cpp @@ -327,7 +327,7 @@ Eigen::MatrixXd OFKTStateSolver::calcJacobianHelper(const std::unordered_map(active_joint_names_.size())); - Eigen::Isometry3d total_tf = Eigen::Isometry3d::Identity(); + Eigen::Isometry3d total_tf{ Eigen::Isometry3d::Identity() }; while (node != root_.get()) { if (node->getType() == JointType::FIXED || node->getType() == JointType::FLOATING) diff --git a/tesseract_urdf/src/origin.cpp b/tesseract_urdf/src/origin.cpp index 93cdd506a4f..b129169cad7 100644 --- a/tesseract_urdf/src/origin.cpp +++ b/tesseract_urdf/src/origin.cpp @@ -90,7 +90,7 @@ Eigen::Isometry3d tesseract_urdf::parseOrigin(const tinyxml2::XMLElement* xml_el Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY()); Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ()); - Eigen::Quaterniond rpy = yawAngle * pitchAngle * rollAngle; + Eigen::Quaterniond rpy{ yawAngle * pitchAngle * rollAngle }; origin.linear() = rpy.toRotationMatrix(); }