1

I'm working in python within the ROS (Robot Operating System).

I have two paths in space defined like a list of x and y coords with a time value for each pair of coords.

Like a 3 tuple of (x,y,t)

Path one (ground_truth) is defined across time very densely with fairly regular spacing. Path two (amcl_pose) can either be sparse or dense depending on where you are on the path, but overall is defined at less points than the ground_truth.

the time vector for the ground truth is called ground_truth_time and the time vector for amcl_pose is called amcl_pose_time and similar names for their x vectors and y vectors:

What I have tried so far is something like:

from scipy.interpolate import griddata;`enter code here`
import numpy as np

method = ['nearest','linear','cubic']
time = np.linspace(0,tf,1000)
gt_x = griddata(ground_truth_time,ground_truth_x,time,method=method[1])
gt_y = griddata(ground_truth_time,ground_truth_y,time,method=method[1])
amcl_x = griddata(amcl_pose_time,amcl_pose_x,time,method=method[1])
amcl_y = griddata(amcl_pose_time,amcl_pose_y,time,method=method[1])

I would then like to calculate the distance between the paths at each point in time. Which would be really easy but something odd is happening.

I found by plotting slices of the paths on top of each other that the beginning of the curves do not lie on top of each other, nor do the ends, and nor do important features throughout the paths that should be near each other.

This is making my distance function which takes a time value and finds the distance between the paths at that time return a much larger number than it should.

This has been the major road block in getting something useful done this summer so any help would be hugely appreciated.

Also please let me know if there's any way I can reformat my question or extra information I should provide to help you help me, this is my first time asking a question here.

EDIT TO SHOW SECOND ATTEMPT IMPLEMENTING ADVICE: *I should point out that gt_t and amcl_t both start at time = 0 and end at nearly the same time: about 100secs. But amcl_t may switch from being sparse to dense throughout the vector it is not equally spaced out.

from scipy.interpolate import interp1d
import numpy as np
from matplotlib import pyplot as plt

#gt_t is an array of time corresponding to the list of points (gt_x,gt_y)
#amcl_t is an array of time corresponding to the list of points(amcl_x,amcl_y)

amcl_x_function = interp1d(amcl_t,amcl_x,kind='linear')
amcl_y_function = interp1d(amcl_t,amcl_y,kind='linear')

amcl_x_interped = amcl_x_function(gt_t)
amcl_y_interped = amcl_y_function(gt_t)

fig = plt.figure()
plt.plot(amcl_x_interped,amcl_y_interped)
plt.plot(gt_x,gt_y)
plt.show()

#this pair of coords are close together which is good 
#since the two paths are always pretty close
print(amcl_x[0],amcl_y[0])
print(gt_x[0],gt_y[0]

#this pair is already so distant from eachother
#and I don't understand why as the two curves are always very close
#before interpolation
print(amcl_x[150],amcl_y[150])
print(gt_x[150],gt_y[150]

The plot this code produces creates two paths that always look very close to eachother. But the time values are so far off that you if you look at the distance between points at a particular point in time you'll find a huge distance. Because one of those points is back on an earlier part of the path while the second one is much further along. They should be near the same point but the interpolation is not working correctly.

Brandon Gaydusek
  • 11
  • 2
  • Solving this would be relatively easy if both paths had the same number of points. To make this so, pick the one with largest number of them and use the time values in it to add appropriately interpolated points with time values in the other where it lacks a corresponding one. After doing this for all the points in the longer path, you should have two paths with the same number of points, all with matching time values. – martineau Aug 05 '16 at 01:46
  • 1
    Okay, so something like interp1d from scipy.interpolate could do that for me I think. I could use that to create a function from my sparse path and evaluate it at the time points from the dense path? I'll try that out and get back to you. – Brandon Gaydusek Aug 05 '16 at 13:12
  • Unfortunately that doesn't seem to work. After interpolation both paths have the same resolution but if I check the two paths at some times across their paths by calling them at certain indices. For instance I'm checking the first element the last and some points in the middle. The first elements are very close but they soon diverge in a way that the pre interpolated data does not. Why is the interpolation ruining the data? Is there any way to fix this? – Brandon Gaydusek Aug 05 '16 at 15:43
  • interp1d might not have worked, but equalizing the paths the way I have suggested still would. It's not that difficult to do manually if you can't find something pre-written in a package. I did something similar a long time ago when I was first learning a new (to me) computer language, so know this first-hand. – martineau Aug 05 '16 at 15:53
  • I appreciate what you're saying but I am not sure how to implement it without using interp1d. I'm sorry for asking but can you show me how to do it? I'm a physics student and coding is not my strongest skill. But I'm doing research this summer in robotics that requires me to stretch out of my comfort zone. Also thank you for such a quick response, it really means alot! I've been beating my head against the wall for weeks on this project and this interpolation problem is at the core of why it's not working. – Brandon Gaydusek Aug 05 '16 at 16:03
  • Sorry, I'm not well versed with the libraries you're using, and as an old timer, think figuring it out yourself would be a good leaning opportunity (as it was for me). If you're a learn-by-example type the question [_Fast linear interpolation in Numpy / Scipy “along a path”_](http://stackoverflow.com/questions/33069366/fast-linear-interpolation-in-numpy-scipy-along-a-path) looks like it might be helpful. – martineau Aug 05 '16 at 16:36
  • P.S. I think that you're going to have to do something about the two paths not ending at the same time. This could be done by linearly extrapolating the shorter one or likewise truncating the longer one so `t` is the same for the end points of each. – martineau Aug 05 '16 at 16:45
  • I have done something about them not ending at the same time around the same time I implemented the advice you first gave me. I needed to do that in order to use interp1d since it requires two arrays of the same length. – Brandon Gaydusek Aug 05 '16 at 17:14
  • I suppose it would be a great learning experience but I'm only at this summer project for another 2 weeks and had already spent several weeks finding this bug and trying to fix it. Thank you for your help you have managed to get me thinking and helped me narrow down further what needs to be done. – Brandon Gaydusek Aug 05 '16 at 17:15
  • In that case I suggest you post a new question asking only about equalizing the two path lists the way I described, showing what you've tried along with some sample input data and the desired output. This will likely produce some useful responses. – martineau Aug 05 '16 at 18:51

0 Answers0