Calculate normal vectors of an octomap node

asked 2019-04-12 05:27:07 -0500

kosmastsk gravatar image

updated 2019-04-12 05:34:15 -0500

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 :)

edit retag flag offensive close merge delete