ROS Answers: Open Source Q&A Forum - RSS feedhttps://answers.ros.org/questions/Open source question and answer forum written in Python and DjangoenROS Answers is licensed under Creative Commons Attribution 3.0Tue, 16 Jun 2015 12:41:07 -0500Calculating Distance From Trajectoryhttps://answers.ros.org/question/211469/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!Tue, 16 Jun 2015 08:12:05 -0500https://answers.ros.org/question/211469/calculating-distance-from-trajectory/Answer by lucasw for <p>Hi all, </p>
<p>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.</p>
<p>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. </p>
<p>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. </p>
<p>Many thanks!</p>
https://answers.ros.org/question/211469/calculating-distance-from-trajectory/?answer=211488#post-id-211488Split 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?Tue, 16 Jun 2015 12:41:07 -0500https://answers.ros.org/question/211469/calculating-distance-from-trajectory/?answer=211488#post-id-211488