Correct error in world2body and body2world

The transformed marker matrix was flatten yielding to uncorrect result due to wrong indexing.
The transformation makes use of pandas dataframes which are also clearer than numpy arrays.

navipy/trajectories/__init__.py

@@ -48,8 +48,14 @@ def _markerstransform(index_i, trajectory,
     trans_mat = htf.compose_matrix(angles=angles,
                                    translate=translate,
                                    axes=rotation_mode)
-    tmarker = trans_mat.dot(homogeneous_markers)[:3]
-    return tmarker.transpose().flatten()
+    tmarker = trans_mat.dot(homogeneous_markers)
+    tmarker = pd.DataFrame(data=tmarker,
+                           index=homogeneous_markers.index,
+                           columns=homogeneous_markers.columns)
+    # We do not need w
+    tmarker = tmarker.loc[['x', 'y', 'z'], :].unstack()
+    tmarker.name = index_i
+    return tmarker
 
 
 def _invmarkerstransform(index_i, trajectory,
@@ -60,8 +66,14 @@ def _invmarkerstransform(index_i, trajectory,
     trans_mat = htf.compose_matrix(angles=angles,
                                    translate=translate,
                                    axes=rotation_mode)
-    tmarker = np.linalg.inv(trans_mat).dot(homogeneous_markers)[:3]
-    return tmarker.transpose().flatten()
+    tmarker = np.linalg.inv(trans_mat).dot(homogeneous_markers)
+    tmarker = pd.DataFrame(data=tmarker,
+                           index=homogeneous_markers.index,
+                           columns=homogeneous_markers.columns)
+    # We do not need w
+    tmarker = tmarker.loc[['x', 'y', 'z'], :].unstack()
+    tmarker.name = index_i
+    return tmarker
 
 
 class Trajectory(pd.DataFrame):
@@ -511,6 +523,11 @@ class Trajectory(pd.DataFrame):
         # The marker are assume to be a multiIndex dataframe
         homogeneous_markers = markers.unstack()
         homogeneous_markers['w'] = 1
+        # Make sure that columns are correctly ordered
+        homogeneous_markers = homogeneous_markers[['x', 'y', 'z', 'w']]
+        # Transpose because we apply homogeneous transformation
+        # on the marker, and thus a 4x4 matrix on a 4xN matrix
+        # here N is the number of markers
         homogeneous_markers = homogeneous_markers.transpose()
         # Looping throught the indeces
         # to get the homogeneous transformation from the position orientation
@@ -522,6 +539,8 @@ class Trajectory(pd.DataFrame):
                         homogeneous_markers=homogeneous_markers,
                         rotation_mode=self.rotation_mode),
                 indeces)
+        # unwrap results
+        indeces = [res.name for res in result]
         transformed_markers = pd.DataFrame(data=result,
                                            index=indeces,
                                            columns=markers.index,
@@ -551,6 +570,11 @@ class Trajectory(pd.DataFrame):
         # The marker are assume to be a multiIndex dataframe
         homogeneous_markers = markers.unstack()
         homogeneous_markers['w'] = 1
+        # Make sure that columns are correctly ordered
+        homogeneous_markers = homogeneous_markers[['x', 'y', 'z', 'w']]
+        # Transpose because we apply homogeneous transformation
+        # on the marker, and thus a 4x4 matrix on a 4xN matrix
+        # here N is the number of markers
         homogeneous_markers = homogeneous_markers.transpose()
         # Looping throught the indeces
         # to get the homogeneous transformation from the position orientation
@@ -562,6 +586,8 @@ class Trajectory(pd.DataFrame):
                         homogeneous_markers=homogeneous_markers,
                         rotation_mode=self.rotation_mode),
                 indeces)
+        # unwrap results
+        indeces = [res.name for res in result]
         transformed_markers = pd.DataFrame(data=result,
                                            index=indeces,
                                            columns=markers.index,