# Calculating Distance From Trajectory

Hi all,

I have a set of points in 3D Space that form a trajectory. My application requires that I generate a 'heatmap' of distances as one moves away from that trajectory.

For example, if one point on the trajectory is [0, 0, 1] (cost 0) then the points away from that trajectory: [1, 0, 1] (cost 1), and [1, 1, 1] (cost 1.414). Whereas if [0, 0, 2] is the next point on the path, a point [0, 0, 1.5] has a cost of 0 associated with it.

Is this possible with a package in ROS for a trajectory? Or even as a library in numpy or scipy? It fits my application to be able to precompute and store this in a data structure, and it does not have to be in real time.

Many thanks!

edit retag close merge delete

Sort by ยป oldest newest most voted

Split up the trajectory into line segments and do 3D line to point distance calculations for all and take the smallest one for every point of interest, optimize it to be smarter later if there are a lot of line segments. Store the points in a 3D matrix (OpenCV, Eigen (?), or a GPU 3d texture format) if it is acceptable to have them in a grid, and you want it to be dense.

Or store the values in a PCL pointcloud if you want them to be located anywhere at all, and pcl has sqrPointToLineDistance already. (And can PCL have a vector fields, a bunch of points with an xyz location each with an xyz vector, or would that require two parallel clouds?)

It seems like you could do it for thousands of points per second real time, don't bother with precomputing dense matrix representations if that is good enough.

Another thought- OpenCV has a way to do this with 2D mats, where you would take a black image, draw the trajectory onto it in white pixels, and then run distanceTransform on it. Maybe it works with 3D mats also?

more