Skip to content

Commit

Permalink
Fix pose
Browse files Browse the repository at this point in the history
  • Loading branch information
GiordanoLaminetti committed Nov 29, 2020
1 parent fb16246 commit 288ddd0
Show file tree
Hide file tree
Showing 4 changed files with 39 additions and 4 deletions.
2 changes: 1 addition & 1 deletion .gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -143,5 +143,5 @@ Dataset/
.ipython
.config
.bash_history

docker_build.sh
results/**
3 changes: 2 additions & 1 deletion slam_method/OrbSlam2.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
from slampy import State
import orbslam2
import os.path
import numpy as np


class Slam:
Expand Down Expand Up @@ -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:
Expand Down
3 changes: 2 additions & 1 deletion slam_method/OrbSlam3.py
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
import orbslam3
import sys
import os.path
import numpy as np

sys.path.append("..")
from slampy import State
Expand Down Expand Up @@ -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:
Expand Down
35 changes: 34 additions & 1 deletion slampy.py
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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()
Expand All @@ -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()
Expand All @@ -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()
Expand All @@ -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()
Expand Down Expand Up @@ -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 .
Expand All @@ -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.
Expand All @@ -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)
Expand All @@ -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
Expand Down

0 comments on commit 288ddd0

Please sign in to comment.