Hey everyone! :)
I am trying to calculate the normal vector of a position in an octomap.
More specifically, let's suppose my robot is in the position A(xa,ya,za)
. Using castRay(...)
, towards the direction (0,0,yaw)
I have found that there is an obstacle in the position B(xb,yb,zb)
.
Now I want to calculate the normal vector of the point B, which means that I will calculate the normal vector of the surface that it belongs to. I used the function getNormals()
from the Octomap API to find it, but it is not always working.
For some points I get 3 or 4 normal vectors, but for some other points, in the same surface, I get 0 normal vectors.
Why is this happening? Is my solution correct or should I follow a different methodology?
Thanks in advance :)