From 219497b15f55e7c29725e270458db75708dc17ed Mon Sep 17 00:00:00 2001
From: filippoaleotti <filippoaleotti@yahoo.it>
Date: Sat, 21 Nov 2020 17:27:33 +0100
Subject: [PATCH] fixed error in dump pose

---
 run.py    |  4 ++--
 slampy.py | 17 +++++++++++------
 2 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/run.py b/run.py
index 80760a4..1f04ac7 100644
--- a/run.py
+++ b/run.py
@@ -47,9 +47,10 @@ def run(args):
             # NOTE: we buid a default invalid depth, in the case of system failure
             h, w = image.shape[:2]
             depth = np.full((h, w), -1, dtype=np.float32)
+            pose_past_frame_to_current = np.full((4, 4), -1, dtype=np.float32)
             if state == slampy.State.OK:
                 depth = app.get_depth()
-
+                pose_past_frame_to_current = app.get_pose(precedent_frame=args.pose_id)
             states.append(state)
 
             name = os.path.splitext(os.path.basename(image_name))[0]
@@ -57,7 +58,6 @@ def run(args):
             save_depth(depth_path, depth)
 
             pose_path = os.path.join(dest_pose, name)
-            pose_past_frame_to_current = app.get_pose(precedent_frame=args.pose_id)
             save_pose(pose_path, pose_past_frame_to_current)
 
             pbar.update(1)
diff --git a/slampy.py b/slampy.py
index a68a307..94ec212 100644
--- a/slampy.py
+++ b/slampy.py
@@ -151,7 +151,7 @@ def process_image_rgbd(self, image, tframe):
             tframe (float): the timestamp when the image was capture
 
         Returns:
-            the state of the traking in this frame
+            the new state of the SLAM system
 
         Raises:
             Exception: if the sensor type is different from RGBD
@@ -176,6 +176,7 @@ def get_pose(self, precedent_frame=-1):
 
         Returns:
             the 4x4 pose matrix corresponding to the transormation between the precedent_frame and the current one.
+            If the state is not State.OK, return None
 
 
         Examples:
@@ -195,6 +196,7 @@ def get_pose(self, precedent_frame=-1):
                     self.slam.get_pose(),
                     np.linalg.inv(self.pose_array[-precedent_frame]),
                 )
+        return None
 
     def get_abs_cloud(self):
         """Get the point cloud at the current frame stored in it's aboslute coordinate .
@@ -205,6 +207,7 @@ def get_abs_cloud(self):
         """
         if self.get_state() == State.OK:
             return self.slam.get_abs_cloud()
+        return None
 
     def get_point_cloud(self):
         """Get the point cloud at the current frame form the wiev of the current position .
@@ -215,6 +218,7 @@ def get_point_cloud(self):
         """
         if self.get_state() == State.OK:
             return [cp for (cp, _) in self._get_2d_point()]
+        return None
 
     def get_depth(self):
         """Get the depth computed in the current image.
@@ -223,17 +227,18 @@ def get_depth(self):
             an array of the image shape with depth when it is aviable otherwise -1 , None if the traking is failed
 
         """
+        depth = None
         if self.get_state() == State.OK:
             depth = np.ones(self.image_shape[0:2]) * -1
             for (cp, point) in self._get_2d_point():
                 depth[point[1], point[0]] = cp[2]
-            return depth
+        return depth
 
     def _get_2d_point(self):
         """This private method is used to compute the transormation between the absolute point to the image point
 
         Return:
-            an array of pair (camera view, image point) , None if the traking is failed
+            an array of pair (camera view, image point) , an empty list if the traking is failed
 
         """
         points2D = []
@@ -258,7 +263,7 @@ def _get_2d_point(self):
         return points2D
 
     def get_state(self):
-        """Return the state of the SLAM tracking in the last frame
+        """Get the current state of the SLAM system
 
         Returns:
             an State enums corresponding to the state
@@ -266,9 +271,9 @@ def get_state(self):
         return self.slam.get_state()
 
     def shutdown(self):
-        """Shutdown the SLAM algorithm"""
+        """Shutdown the SLAM system"""
         self.slam.shutdown()
 
     def reset(self):
-        """Reset SLAM algorithm"""
+        """Reset SLAM system"""
         self.slam.reset()