Merge branch 'develop' of gitlab.ub.uni-bielefeld.de:olivier.bertrand/navipy into develop

doc/source/tutorials/03-rendering-along-trajectory.ipynb

@@ -168,7 +168,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.6.3"
+   "version": "3.6.5"
   }
  },
  "nbformat": 4,

navipy/scripts/blend_overlaytraj.py

@@ -8,11 +8,15 @@ import inspect
 import pkg_resources
 import tempfile
 # Following need to be imported in blender as well
-from navipy.tools.trajectory import Trajectory
+from navipy.trajectories import Trajectory
+from navipy.maths.homogeneous_transformations import compose_matrix
+from navipy.maths.quaternion import matrix as quatmatrix
 
 
 importwithinblender = [
-    'from navipy.tools.trajectory import Trajectory']
+    'from navipy.trajectories import Trajectory',
+    'from navipy.maths.homogeneous_transformations import compose_matrix',
+    'from navipy.maths.quaternion import matrix as quatmatrix']
 
 
 def parser_blend_overlaytraj():
@@ -20,9 +24,9 @@ def parser_blend_overlaytraj():
     parser = argparse.ArgumentParser()
     arghelp = 'Path to the environment (.blend) in which your agent lives'
     defaultworld = pkg_resources.resource_filename(
-        'navipy', 'resources/corridor.blend')
+        'navipy', 'resources/sample_experiment/Ravi_2018/corridor.blend')
     defaulttraj = pkg_resources.resource_filename(
-        'navipy', 'resources/corridor_traj.csv')
+        'navipy', 'resources/sample_experiment/Ravi_2018/corridor_traj.csv')
     parser.add_argument('--blender-world',
                         type=str,
                         default=defaultworld,
@@ -81,6 +85,59 @@ def run(trajfile):
     #bpy.context.object.data.resolution_u = 3
     #bpy.context.object.data.bevel_object = bpy.data.objects["NurbsCircle"]
 
+    # add frame
+    bpy.ops.object.empty_add(type='ARROWS',
+                             radius=1,
+                             view_align=False,
+                             location=(0, 0, 0),
+                             layers=(True, False, False, False, False,
+                                     False, False, False, False, False,
+                                     False, False, False, False, False,
+                                     False, False, False, False, False))
+    frameobject = bpy.context.object
+    frameobject.name = "camera_frame"
+    # find camera:
+    # to then move it on each frame
+    camera_found = False
+    for obj in bpy.context.scene.objects:
+        if obj.type == 'CAMERA':
+            camera = obj
+            camera_found = True
+            break
+    if not camera_found:
+        raise NameError('The blender file does not contain a camera')
+    # set first and last frame
+    context = bpy.context
+    context.scene.frame_start = trajectory.index.min()
+    context.scene.frame_end = trajectory.index.max()
+    keyInterp = context.user_preferences.edit.keyframe_new_interpolation_type
+    context.user_preferences.edit.keyframe_new_interpolation_type = 'LINEAR'
+    convention = trajectory.rotation_mode
+    _renderaxis = '+x'
+    for frame_i, posorient in trajectory.iterrows():
+        context.scene.frame_current = frame_i
+        # Render
+        rotmat = compose_matrix(
+            angles=posorient.loc[convention],
+            translate=posorient.loc['location'],
+            axes=convention)
+        frameobject.matrix_world = rotmat.transpose()
+        if _renderaxis == '+x':
+            initrot = quatmatrix([0.5, 0.5, -0.5, -0.5])
+            # The camera is aligned in -z
+            # A rotation along z wll thus roll the camera
+            # Althougth the camera should yaw in such case
+            rotmat[:3, :3] = rotmat[:3, :3].dot(initrot[:3, :3])
+        # matrix_world in blender are column-major order
+        # and numpy row-major order
+        camera.matrix_world = rotmat.transpose()
+        camera.keyframe_insert(data_path='location', frame=(frame_i))
+        camera.keyframe_insert(data_path='rotation_euler', frame=(frame_i))
+        frameobject.keyframe_insert(data_path='location', frame=(frame_i))
+        frameobject.keyframe_insert(
+            data_path='rotation_euler', frame=(frame_i))
+    context.user_preferences.edit.keyframe_new_interpolation_type = keyInterp
+
 
 def main():
     # encoding for temporary file

navipy/sensors/renderer.py

@@ -569,7 +569,10 @@ class BlenderRender(AbstractRender):
             axes=convention)
         if self._renderaxis == '+x':
             initrot = quatmatrix([0.5, 0.5, -0.5, -0.5])
-            rotmat[:3, :3] = initrot[:3, :3].dot(rotmat[:3, :3])
+            # The camera is aligned in -z
+            # A rotation along z wll thus roll the camera
+            # Althougth the camera should yaw in such case
+            rotmat[:3, :3] = rotmat[:3, :3].dot(initrot[:3, :3])
         # matrix_world in blender are column-major order
         # and numpy row-major order
         self.camera.matrix_world = np.transpose(rotmat)

navipy/trajectories/__init__.py

@@ -233,6 +233,7 @@ class Trajectory(pd.DataFrame):
     def q_3(self, q_3):
         self.__set_q_i(3, q_3)
 
+    # overload of save/load function
     def read_csv(self, filename, sep=',', header=[0, 1], index_col=0):
         """ Load from a hdf file
         """
@@ -242,8 +243,19 @@ class Trajectory(pd.DataFrame):
         return self
 
     def read_hdf(self, filename):
-        raise NameError('Not implemented')
+        df = pd.read_hdf(filename)
+        self.from_dataframe(df)
+        return self
+
+    def to_hdf(self, filename):
+        df = pd.DataFrame(self)
+        df.to_hdf(filename, key='posorients')
+
+    def to_csv(self, filename):
+        df = pd.DataFrame(self)
+        df.to_csv(filename)
 
+    # initialisation from variables
     def from_array(self, nparray, rotconv):
         """ Assign trajectory from a numpy array
             N x 6 (rotconv = Euler angles)