Rosanswers logo


I am trying to generate the starting points and the ending points of the following arrows: image description

The idea is that I get the start (yellow, 0,0,2) and the end (orange 1, 1, 5).
The arrows are the path of ray tracing to find obstacles.
These arrows should all be parallel amongst themselves but also orthogonal to the vector start to goal. The search should be within a given safety margin.

I given a detailed description of what I've done bellow but here is what I wanted to ask Since we can specify frames just by giving the offset and the angles and then tf can calculate the transform, I thought there might be a way to create dynamic frames and then use the same functions as tf to find the transform. Is this possible?

The approach I used to generate the image was to imagine a 2D square (over x and z) around both start and goal. Then to find the yaw and roll that make the squares parallel and perform the rotation. This approach fails when the points need to be generated over both x and y ( you can see in the picture that square should be rotated in the direction of the camera). The code is at https://github.com/margaridaCF/FlyingOctomap_code/blob/d56b1193d177eef08e0626998e6b54fa0d0a2dc9/path_planning/src/ltStar_lib.cpp#L443

Then I thought of generating the points in an auxiliary frame at the origin. Always generating the points in x and z and then rotating them back. This would be done combining the following

    final_transform_start = translation_from_center_start * rotation_roll * rotation_yaw;
    final_transform_end   = translation_from_center_end   * rotation_roll * rotation_yaw;

In this approach, I run into issues with angles being interpreted clockwise and counterclockwise (I think) and also how to handle 90º yaw and roll. The code can be found here https://github.com/margaridaCF/FlyingOctomap_code/blob/20180814_filterNeighborsAtGeneration/path_planning/src/ltStar_lib.cpp#L493

UPDATE: (Building on the answer from @PeteBlackerThe3rd )

I started with this process but got hanged on point 3 because there was too much freedom to choose and I couldn't figure out how to decide when I got the right one.

Later a colleague pointed out that all I need to do is check if the norm of the normalization of the dot product of the axis with the direction vector is below 0.9 (more or less).
When it is 1 they are parallel and when it's over ~0.9 the cross product will lose accuracy.
If this is the case just use another unit axis. As unit axis are orthogonal, if it is close to parallel in one axis it won't be in the others.

| direction * î | < ~0.9

Originally posted by kotoko on ROS Answers with karma: 122 on 2018-08-20

Post score: 0

Original comments

Comment by PeteBlackerThe3rd on 2018-08-20:
Can you describe the actual problem you're trying to solve, the description is very abstract. It sounds as though your rotating and translating sets of points, which can be achieved using transform objects without any published TF frames.

Comment by kotoko on 2018-08-20:
@PeteBlackerThe3rd Rewrote the question to be more specific. Thanks for the feedback.


1 Answer 1


Rosanswers logo

Okay, if I can summarise what you've said to make sure I understand it properly. The input to your process is a vector defined by it's start and end points. From this information you want to generate a set of vectors parallel to this input vector but spaced in a square grid formation if viewed along the direction of the input vector. Is this correct?

As you've found the main challenge here is finding the two direction vectors which are orthogonal to the input vector, when you have these you can easily create the sets of start and end points you want.

If I've understood the problem correctly then it's not fully defined, the rotation of the square grids around the input vector is not determined. So the solution I'm about to describe assumes any rotation is acceptable.

The tool we're going to use is the vector cross product, if you calculate this product for any non-parallel vectors the result will be a vector orthogonal to both input vectors with a length which is the scalar produce of the lengths of the two input vectors. This is available to you in the tf::Vector3::Cross method.

  1. First find the direction vector of your input by subtracting the start point from the end point.
  2. Normalise this direction vector (divide it by it's length) so that it's a unit vector.
  3. Choose an axis unit vector (x, y, or z) which is not parallel to the input vector, it doesn't matter which.
  4. Calculate the cross product of this axis vector with the direction vector, the result is the first orthogonal vector for your grid of points.
  5. Finally calculate the cross product of the first orthogonal vector with the direction vector, the result is the second orthogonal vector for the grid.

You can now use the two orthogonal direction vectors along with the original start and end points to create the grid of vectors you need. Hope this helps you get this working.

Originally posted by PeteBlackerThe3rd with karma: 9529 on 2018-08-23

This answer was ACCEPTED on the original site

Post score: 1

Original comments

Comment by kotoko on 2018-08-23:
Thank you so much! I was going for a much more complicated way to solve this (with translations and rotations). There is one detail that I stumble on in your answer, I can't post it as a comment so I'll add as an answer and you can merge if you want.

Comment by PeteBlackerThe3rd on 2018-08-23:
Your colleague is correct any of the axis unit vectors (well actually any unit vector at all) which has a dot product less than 0.9 with your direction vector will be fine. There are multiple 'right' answers to this one! Let me know if you have any problems getting this working.

Comment by PeteBlackerThe3rd on 2018-08-23:
I've merged your response with the original question.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.