diff --git a/navipy/trajectories/__init__.py b/navipy/trajectories/__init__.py index 1f2856db9bb50e336d39739df671a43d2623f6c6..fce3e80a00ebe32639979adbf66ada510bfa52f2 100644 --- a/navipy/trajectories/__init__.py +++ b/navipy/trajectories/__init__.py @@ -640,9 +640,57 @@ class Trajectory(pd.DataFrame): 'dalpha_2']] = rot.squeeze() return velocity + def traveled_distance(self): + """ Calculate the travel distance + + Note that Nans are linearly interpolated + """ + # We remove nans between section + # and then calculate the velocity + # it is equivalent to interpolate between non-nan blocks + subtraj = self.location.dropna().reset_index().drop('index', + axis=1, level=0) + if subtraj.dropna().shape[0] < 2: + print('Trajectory has less than 2 non nans points') + return np.nan + velocity = subtraj.diff() # only location is of relevance + speed = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) + travel_dist = np.sum(speed) + return travel_dist + + def sinuosity(self, shortest_dist=None): + """ Calculate the sinosity + Sinusity is defined as: + Travelled distance + S=-------------------- + Shortest distance + + Note that Nans are linearly interpolated + + :param shortest_dist: Assign shortest distance (default\ + None the shortest distance is equal to the bee line between \ +first and last no-nan point. + """ + travel_dist = self.traveled_distance() + if shortest_dist is None: + # we need to calculate the shortest distance + # assuming the direct line + firstpoint = self.location.dropna().iloc[0, :] + lastpoint = self.location.dropna().iloc[-1, :] + shortest_dist = lastpoint-firstpoint + shortest_dist = np.sqrt( + shortest_dist.x**2 + shortest_dist.y**2 + shortest_dist.z**2) + # sanity check + # the travelled distance can not be shorter than the shortest distance + if shortest_dist > travel_dist: + msg = "Travel distance is shorter than the shortest distance" + msg += "\n {}>{}".format(shortest_dist, travel_dist) + raise NameError(msg) + return travel_dist/shortest_dist # -------------------------------------------- # ---------------- FILTER -------------------- # -------------------------------------------- + def filtfilt(self, order, cutoff, padlen=None): """ Filter the trajectory with order and cutoff by diff --git a/navipy/trajectories/test_trajectory.py b/navipy/trajectories/test_trajectory.py index 4e624d9f2f7f99e68a6b00926d2a3a2337cf4e5c..96ec2a725aa6ba04f83a9e5f4314e2d738e5586c 100644 --- a/navipy/trajectories/test_trajectory.py +++ b/navipy/trajectories/test_trajectory.py @@ -53,6 +53,40 @@ class TestTrajectoryTransform(unittest.TestCase): def test_velocity(self): pass + def test_traveldist(self): + indeces = np.linspace(0, 2*np.pi, 1000) + radius = 5 + mytraj = Trajectory(indeces=indeces, rotconv='zyx') + mytraj.x = indeces + mytraj.y = radius*np.cos(indeces) + mytraj.z = 0 + # The length of a cos from 0 to 2pi + # is equal to perimeter of the circle + # 2*pi*r + travel_dist_theo = 2*np.pi*radius + travel_dist = mytraj.traveled_distance() + np.testing.assert_almost_equal(travel_dist, travel_dist_theo) + # Test with nans + mytraj.loc[[15, 50, 90], :] = np.nan + travel_dist = mytraj.traveled_distance() + np.testing.assert_almost_equal(travel_dist, travel_dist_theo) + + def test_sinuosity(self): + indeces = np.linspace(0, 2*np.pi, 1000) + radius = 5 + mytraj = Trajectory(indeces=indeces, rotconv='zyx') + mytraj.x = indeces + mytraj.y = radius*np.cos(indeces) + mytraj.z = 0 + # The length of a cos from 0 to 2pi + # is equal to perimeter of the circle + # 2*pi*r + # the sinuosity will be equal to the radius + # because dist from start to end is 2*pi + sinuosity_theo = radius + sinuosity = mytraj.sinuosity() + np.testing.assert_almost_equal(sinuosity, sinuosity_theo) + if __name__ == '__main__': unittest.main()