Skip to content

Commit

Permalink
Performance improvements found using callgrind (#852)
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Mar 1, 2023
1 parent ef5c5fa commit fe7e78c
Show file tree
Hide file tree
Showing 9 changed files with 49 additions and 23 deletions.
2 changes: 1 addition & 1 deletion tesseract_common/include/tesseract_common/yaml_utils.h
View file
Expand Up @@ -295,7 +295,7 @@ struct convert<Eigen::Isometry3d>
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();
}
Expand Down
4 changes: 2 additions & 2 deletions tesseract_common/src/utils.cpp
View file
Expand Up @@ -133,7 +133,7 @@ Eigen::Vector3d calcRotationalError(const Eigen::Ref<const Eigen::Matrix3d>& 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;
Expand All @@ -157,7 +157,7 @@ Eigen::Vector3d calcRotationalError2(const Eigen::Ref<const Eigen::Matrix3d>& 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;
Expand Down
4 changes: 4 additions & 0 deletions tesseract_geometry/include/tesseract_geometry/mesh_parser.h
View file
Expand Up @@ -83,6 +83,7 @@ std::vector<std::shared_ptr<T>> extractMeshData(const aiScene* scene,
bool material_and_texture)
{
std::vector<std::shared_ptr<T>> meshes;
meshes.reserve(node->mNumMeshes);

aiMatrix4x4 transform = parent_transform;
transform *= node->mTransformation;
Expand All @@ -96,6 +97,7 @@ std::vector<std::shared_ptr<T>> extractMeshData(const aiScene* scene,
std::shared_ptr<std::vector<MeshTexture::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];
Expand Down Expand Up @@ -129,6 +131,7 @@ std::vector<std::shared_ptr<T>> extractMeshData(const aiScene* scene,
if (normals && a->HasNormals())
{
vertex_normals = std::make_shared<tesseract_common::VectorVector3d>();
vertex_normals->reserve(a->mNumVertices);
for (unsigned int i = 0; i < a->mNumVertices; ++i)
{
aiVector3D v = transform * a->mNormals[i];
Expand All @@ -141,6 +144,7 @@ std::vector<std::shared_ptr<T>> extractMeshData(const aiScene* scene,
if (vertex_colors && a->HasVertexColors(0))
{
vertex_colors = std::make_shared<tesseract_common::VectorVector4d>();
vertex_colors->reserve(a->mNumVertices);
for (unsigned int i = 0; i < a->mNumVertices; ++i)
{
aiColor4D v = a->mColors[0][i];
Expand Down
9 changes: 4 additions & 5 deletions tesseract_kinematics/core/include/tesseract_kinematics/core/utils.h
View file
Expand Up @@ -62,8 +62,7 @@ inline static void numericalJacobian(Eigen::Ref<Eigen::MatrixXd> 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<int>(joint_values.size()); ++i)
{
Expand All @@ -72,13 +71,13 @@ inline static void numericalJacobian(Eigen::Ref<Eigen::MatrixXd> 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);
Expand Down
43 changes: 33 additions & 10 deletions tesseract_scene_graph/src/kdl_parser.cpp
View file
Expand Up @@ -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;
}
Expand Down
2 changes: 1 addition & 1 deletion tesseract_srdf/src/group_tool_center_points.cpp
View file
Expand Up @@ -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();
}
Expand Down
4 changes: 2 additions & 2 deletions tesseract_state_solver/src/kdl_state_solver.cpp
View file
Expand Up @@ -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
{
Expand Down Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion tesseract_state_solver/src/ofkt_state_solver.cpp
View file
Expand Up @@ -327,7 +327,7 @@ Eigen::MatrixXd OFKTStateSolver::calcJacobianHelper(const std::unordered_map<std
OFKTNode* node = link_map_.at(link_name);
Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, static_cast<Eigen::Index>(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)
Expand Down
2 changes: 1 addition & 1 deletion tesseract_urdf/src/origin.cpp
View file
Expand Up @@ -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();
}
Expand Down

0 comments on commit fe7e78c

Please sign in to comment.