# 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!