Is there a library or function for calculating distance/angles between models?
I need to calculate the real, not estimated, distance and angles between two models in Gazebo. I need to know what angle the second model is in the z and y direction from the first model, using the first models orientation.
To better explain with an example of what I'm doing, if I have a quadcopter with a camera facing in any direction, and there is another model in the camera, what is the real angle between where the quadcopter is currently facing and the object in the left/right direction of the camera and what is the real angle between where the quadcopter is currently facing and the object in the up/down direction of the camera.
This is for comparison purposes to angles estimated from the image taken from the camera. I need them to be as close to exact as possible.
I was hoping there was already some package or functions for this. I looked at the gazebo/math directory, and I see some functions there to help with the calculations, but not one that does the complete calculation of angles/distances between two given positions/orientations.
So, are there any packages/functions that do that? Or, will I need to figure out the calculations from scratch?
Thanks.
Asked by lucas on 2015-12-15 15:01:36 UTC
Answers
The angle between angle between two 3D positions around a specific axis can be found with:
For yaw(about Z-axis)
atan2(dx, dy)
For pitch(about Y-axis)
atan2(dz, dx)
where dx, dy, and dz are the distances between the two models along the respective axis.
You should check that math.
Asked by nkoenig on 2015-12-16 11:56:40 UTC
Comments