Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Small matrix calc speedup in collision_distance_field_types #666

Merged
merged 4 commits into from
Oct 2, 2021
Merged

Small matrix calc speedup in collision_distance_field_types #666

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
6a71f96
Use transpose of rotation matrix in collision_distance_field_types
Sep 5, 2021
37c5c03
Add comment
Sep 5, 2021
7c1df24
Merge branch 'main' into andyz/use_transpose_when_possible
henningkayser Sep 28, 2021
2f26a02
Merge branch 'main' into andyz/use_transpose_when_possible
Oct 2, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion ...n_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -160,7 +160,8 @@ class PosedDistanceField : public distance_field::PropagationDistanceField
double getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z,
bool& in_bounds) const
{
Eigen::Vector3d rel_pos = pose_.inverse() * Eigen::Vector3d(x, y, z);
// Transpose of a rotation matrix equals its inverse, but computationally cheaper
Eigen::Vector3d rel_pos = pose_.linear().transpose() * Eigen::Vector3d(x, y, z);
double gx, gy, gz;
double res = distance_field::PropagationDistanceField::getDistanceGradient(rel_pos.x(), rel_pos.y(), rel_pos.z(),
gx, gy, gz, in_bounds);
Expand Down