From 288ddd0eb4b4ff312e1b0c8fde065e825c4bfd4d Mon Sep 17 00:00:00 2001
From: Giordano Laminetti <giordano.laminetti.gl@gmail.com>
Date: Sun, 29 Nov 2020 20:36:58 +0100
Subject: [PATCH] Fix pose

---
 .gitignore              |  2 +-
 slam_method/OrbSlam2.py |  3 ++-
 slam_method/OrbSlam3.py |  3 ++-
 slampy.py               | 35 ++++++++++++++++++++++++++++++++++-
 4 files changed, 39 insertions(+), 4 deletions(-)

diff --git a/.gitignore b/.gitignore
index 8ffd4fd..56bff60 100644
--- a/.gitignore
+++ b/.gitignore
@@ -143,5 +143,5 @@ Dataset/
 .ipython
 .config
 .bash_history
-
+docker_build.sh
 results/**
\ No newline at end of file
diff --git a/slam_method/OrbSlam2.py b/slam_method/OrbSlam2.py
index 26f8374..4ba9eba 100644
--- a/slam_method/OrbSlam2.py
+++ b/slam_method/OrbSlam2.py
@@ -5,6 +5,7 @@
 from slampy import State
 import orbslam2
 import os.path
+import numpy as np
 
 
 class Slam:
@@ -67,7 +68,7 @@ def process_image_rgbd(self, image, tframe):
 
     def get_pose(self):
         if self.slam.get_tracking_state() == orbslam2.TrackingState.OK:
-            return self.slam.get_frame_pose()
+            return np.linalg.inv(self.slam.get_frame_pose())
 
     def get_abs_cloud(self):
         if self.slam.get_tracking_state() == orbslam2.TrackingState.OK:
diff --git a/slam_method/OrbSlam3.py b/slam_method/OrbSlam3.py
index fe6cb6d..62f74be 100644
--- a/slam_method/OrbSlam3.py
+++ b/slam_method/OrbSlam3.py
@@ -1,6 +1,7 @@
 import orbslam3
 import sys
 import os.path
+import numpy as np
 
 sys.path.append("..")
 from slampy import State
@@ -85,7 +86,7 @@ def process_image_rgbd(self, image, tframe):
 
     def get_pose(self):
         if self.slam.get_tracking_state() == orbslam3.TrackingState.OK:
-            return self.slam.get_frame_pose()
+            return np.linalg.inv(self.slam.get_frame_pose())
 
     def get_abs_cloud(self):
         if self.slam.get_tracking_state() == orbslam3.TrackingState.OK:
diff --git a/slampy.py b/slampy.py
index 94ec212..fd13128 100644
--- a/slampy.py
+++ b/slampy.py
@@ -70,6 +70,7 @@ def process_image_mono(self, image, tframe):
         """
         self.image_shape = image.shape
         self.slam.process_image_mono(image, tframe)
+        self.image = image
         if self.get_state() == State.OK:
             self.pose_array.append(self.get_pose())
         return self.get_state()
@@ -92,6 +93,7 @@ def process_image_stereo(self, image_left, image_right, tframe):
         """
         self.image_shape = image_left.shape
         self.slam.process_image_stereo(image_left, image_right, tframe)
+        self.image = image
         if self.get_state() == State.OK:
             self.pose_array.append(self.get_pose())
         return self.get_state()
@@ -114,6 +116,7 @@ def process_image_imu_mono(self, image, tframe, imu):
         """
         self.image_shape = image.shape
         self.slam.process_image_imu_mono(image, tframe, imu)
+        self.image = image
         if self.get_state() == State.OK:
             self.pose_array.append(self.get_pose())
         return self.get_state()
@@ -137,6 +140,7 @@ def process_image_imu_stereo(self, image_left, image_right, tframe, imu):
         """
         self.image_shape = image_left.shape
         self.slam.process_image_imu_stereo(image_left, image_right, tframe, imu)
+        self.image = image
         if self.get_state() == State.OK:
             self.pose_array.append(self.get_pose())
         return self.get_state()
@@ -159,6 +163,7 @@ def process_image_rgbd(self, image, tframe):
         """
         self.image_shape = image.shape
         self.slam.process_image_rgbd(image, tframe)
+        self.image = image
         if self.get_state() == State.OK:
             self.pose_array.append(self.get_pose())
         return self.get_state()
@@ -198,6 +203,12 @@ def get_pose(self, precedent_frame=-1):
                 )
         return None
 
+    def get_pose_inverse(self):
+        """Get the inverse frame pose between the frame 0 and the current one."""
+        if self.get_state() == State.OK:
+            return np.linalg.inv(self.slam.get_pose())
+        return None
+
     def get_abs_cloud(self):
         """Get the point cloud at the current frame stored in it's aboslute coordinate .
 
@@ -220,6 +231,20 @@ def get_point_cloud(self):
             return [cp for (cp, _) in self._get_2d_point()]
         return None
 
+    def get_point_cloud_colored(self):
+        """Get the point cloud at the current frame form the wiev of the current position with the RGB color of the point .
+
+        Return:
+            an array with the 3D coordinate of the point and the relative RGB color, None if the traking is failed
+
+        """
+        if self.get_state() == State.OK:
+            return [
+                [cp, self.image[point[1], point[0]]]
+                for (cp, point) in self._get_2d_point()
+            ]
+        return None
+
     def get_depth(self):
         """Get the depth computed in the current image.
 
@@ -244,7 +269,7 @@ def _get_2d_point(self):
         points2D = []
         points = self.get_abs_cloud()
         camera_matrix = self.slam.get_camera_matrix()
-        pose = self.get_pose()
+        pose = self.get_pose_inverse()
         for point in points:
             point = np.append(point, [1]).reshape(4, 1)
             camera_points = np.dot(pose, point)
@@ -262,6 +287,14 @@ def _get_2d_point(self):
                     points2D.append([camera_points, (int(u), int(v))])
         return points2D
 
+    def get_camera_matrix(self):
+        """Get the instrinsec parameter of camera
+
+        Returns:
+            the camera matrix
+        """
+        return self.slam.get_camera_matrix()
+
     def get_state(self):
         """Get the current state of the SLAM system