Merge pull request facebookresearch#82 from soumith/Develop

add implementation of pix_to_3dpt for Habitat backend

robots/LoCoBot/locobot_navigation/orb_slam2_ros/cfg/realsense_habitat.yaml

@@ -6,17 +6,17 @@
 
 Camera.fx: 1000
 Camera.fy: 1000
-Camera.cx: 320.0
-Camera.cy: 240.0
+Camera.cx: 256
+Camera.cy: 256
 
 Camera.k1: 0
 Camera.k2: 0
 Camera.p1: 0
 Camera.p2: 0
 Camera.k3: 0
 
-Camera.width: 640
-Camera.height: 480
+Camera.width: 512
+Camera.height: 512
 
 # Camera frames per second
 Camera.fps: 30.0

src/pyrobot/habitat/base.py

@@ -43,6 +43,8 @@ def get_state(self, state_type="odom"):
 
         init_rotation = self._rot_matrix(self.init_state.rotation)
 
+        # true position here refers to the relative position from
+        # where `self.init_state` is treated as origin
         true_position = cur_state.position - self.init_state.position
         true_position = np.matmul(init_rotation.transpose(), true_position)
 
@@ -52,8 +54,8 @@ def get_state(self, state_type="odom"):
         (r, pitch, yaw) = euler_from_matrix(cur_rotation, axes="sxzy")
         # Habitat has y perpendicular to map where as ROS has z perpendicular
         # to the map. Where as x is same.
-        # Here ROS_Z = -1 * habitat_z and ROS_Y = habitat_x
-        return (-1 * true_position[2], true_position[0], yaw)
+        # Here ROS_X = -1 * habitat_z and ROS_Y = -1*habitat_x
+        return (-1 * true_position[2], -1* true_position[0], yaw)
 
     def stop(self):
         raise NotImplementedError("Veclocity control is not supported in Habitat-Sim!!")
@@ -174,9 +176,9 @@ def _go_to_relative_pose(self, rel_x, rel_y, abs_yaw):
 
         if math.sqrt(rel_x ** 2 + rel_y ** 2) > 0.0:
             # rotate to point to (x, y) point
-            action_name = "turn_right"
+            action_name = "turn_left"
             if rel_y < 0.0:
-                action_name = "turn_left"
+                action_name = "turn_right"
 
             v1 = np.asarray([1, 0])
             v2 = np.asarray([rel_x, rel_y])

src/pyrobot/habitat/camera.py

@@ -15,6 +15,8 @@
 
 import habitat_sim.agent as habAgent
 import habitat_sim.utils as habUtils
+from pyrobot.locobot.camera import DepthImgProcessor
+from tf.transformations import euler_from_quaternion, euler_from_matrix
 
 
 class LoCoBotCamera(object):
@@ -26,73 +28,129 @@ def __init__(self, configs, simulator):
         self.agent = self.sim.get_agent(self.configs.COMMON.SIMULATOR.DEFAULT_AGENT_ID)
 
         # Depth Image processor
+        self.depth_cam = DepthImgProcessor(
+            subsample_pixs=1,
+            depth_threshold=(0, 100),
+            cfg_filename="realsense_habitat.yaml"
+        )
 
         # Pan and tilt related vairades.
         self.pan = 0.0
         self.tilt = 0.0
 
     def get_rgb(self):
         observations = self.sim.get_sensor_observations()
-        return observations["rgb"]
+        return observations["rgb"][:, :, 0:3]
 
     def get_depth(self):
         observations = self.sim.get_sensor_observations()
         return observations["depth"]
 
     def get_rgb_depth(self):
         observations = self.sim.get_sensor_observations()
-        return observations["rgb"], observations["depth"]
+        return observations["rgb"][:, :, 0:3], observations["depth"]
 
     def get_intrinsics(self):
-
-        # Todo: Remove this after we fix intrinsics
-        raise NotImplementedError
         """
 		Returns the instrinsic matrix of the camera
 
 		:return: the intrinsic matrix (shape: :math:`[3, 3]`)
 		:rtype: np.ndarray
 		"""
-        # fx = self.configs['Camera.fx']
-        # fy = self.configs['Camera.fy']
-        # cx = self.configs['Camera.cx']
-        # cy = self.configs['Camera.cy']
+        fx = self.configs['Camera.fx']
+        fy = self.configs['Camera.fy']
+        cx = self.configs['Camera.cx']
+        cy = self.configs['Camera.cy']
         Itc = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
         return Itc
 
-    def pix_to_3dpt(self, rs, cs, in_cam=False):
+    def pix_to_3dpt(self, rs, cs, in_cam=False, initial_rotation=None):
         """
-		Get the 3D points of the pixels in RGB images.
-
-		:param rs: rows of interest in the RGB image.
-		           It can be a list or 1D numpy array
-		           which contains the row indices.
-		           The default value is None,
-		           which means all rows.
-		:param cs: columns of interest in the RGB image.
-		           It can be a list or 1D numpy array
-		           which contains the column indices.
-		           The default value is None,
-		           which means all columns.
-		:param in_cam: return points in camera frame,
-		               otherwise, return points in base frame
+        Get the 3D points of the pixels in RGB images.
+
+        :param rs: rows of interest in the RGB image.
+                   It can be a list or 1D numpy array
+                   which contains the row indices.
+                   The default value is None,
+                   which means all rows.
+        :param cs: columns of interest in the RGB image.
+                   It can be a list or 1D numpy array
+                   which contains the column indices.
+                   The default value is None,
+                   which means all columns.
+        :param in_cam: return points in camera frame,
+                       otherwise, return points in base frame
+
+        :param initial_rotation: a known initial rotation of the camera sensor
+                                 to calibrate habitat origin. The default value
+                                 is None which means it uses the Habitat-reported
+                                 value.
+
+        :type rs: list or np.ndarray
+        :type cs: list or np.ndarray
+        :type in_cam: bool
+
+        :returns: tuple (pts, colors)
+
+                  pts: point coordinates in world frame
+                  (shape: :math:`[N, 3]`)
+
+                  colors: rgb values for pts_in_cam
+                  (shape: :math:`[N, 3]`)
+
+        :rtype: tuple(np.ndarray, np.ndarray)
+        """
+        rgb_im, depth_im = self.get_rgb_depth()
+        pts_in_cam = self.depth_cam.get_pix_3dpt(depth_im=depth_im, rs=rs, cs=cs)
+        pts = pts_in_cam[:3, :].T
+
+        colors = rgb_im[rs, cs].reshape(-1, 3)
+        if in_cam:
+            return pts, colors
+
+        # here, points are now given in camera frame
+        # the thing to do next is to transform the points from camera frame into the
+        # global frame of pyrobot environment
+        # This does not translate to what habitat thinks as origin,
+        # because pyrobot's habitat-reference origin is `self.agent.init_state`
+        # So, CAMERA frame -> HABITAT frame -> PYROBOT frame
+
+        init_state = self.agent.initial_state # habitat - x,y,z
+        cur_state = self.agent.get_state()
+        cur_sensor_state = cur_state.sensor_states['rgb']
+        if initial_rotation is None:
+            initial_rotation = init_state.sensor_states['rgb'].rotation
+        rot_init_rotation = self._rot_matrix(initial_rotation)
 
-		:type rs: list or np.ndarray
-		:type cs: list or np.ndarray
-		:type in_cam: bool
 
-		:returns: tuple (pts, colors)
+        ros_to_habitat_frame = np.array([[ 0.0, -1.0,  0.0],
+                                         [ 0.0,  0.0,  1.0],
+                                         [-1.0,  0.0,  0.0]])
 
-		          pts: point coordinates in world frame
-		          (shape: :math:`[N, 3]`)
+        camera_to_image_frame = np.array([[1.0,  0.0,  0.0],
+                                          [0.0, -1.0,  0.0],
+                                          [0.0,  0.0, -1.0]])
 
-		          colors: rgb values for pts_in_cam
-		          (shape: :math:`[N, 3]`)
+        relative_position = cur_sensor_state.position - init_state.position
+        relative_position = rot_init_rotation.T @ relative_position
 
-		:rtype: tuple(np.ndarray, np.ndarray)
-		"""
+        cur_rotation = self._rot_matrix(cur_sensor_state.rotation)
+        cur_rotation = rot_init_rotation.T @ cur_rotation
+
+        rot_habitat_origin_to_image_frame = cur_rotation @ camera_to_image_frame
+
+        # now do the final transformation and return the points
+        pts = np.dot(pts, rot_habitat_origin_to_image_frame.T)
+        pts = pts + relative_position
+        pts = ros_to_habitat_frame.T @ pts.T
+        pts = pts.T
+        return pts, colors
+
+
+    def _rot_matrix(self, habitat_quat):
+        quat_list = [habitat_quat.x, habitat_quat.y, habitat_quat.z, habitat_quat.w]
+        return prutil.quat_to_rot_mat(quat_list)
 
-        raise NotImplementedError
 
     def get_current_pcd(self, in_cam=True):
         """
@@ -189,7 +247,7 @@ def _compute_relative_pose(self, pan, tilt):
         sensor_offset_tilt = np.asarray([0.0, 0.0, -1 * tilt_link])
 
         quat_cam_to_pan = habUtils.quat_from_angle_axis(
-            -1 * self.tilt, np.asarray([1.0, 0.0, 0.0])
+            -1 * tilt, np.asarray([1.0, 0.0, 0.0])
         )
 
         sensor_offset_pan = habUtils.quat_rotate_vector(
@@ -198,7 +256,7 @@ def _compute_relative_pose(self, pan, tilt):
         sensor_offset_pan += np.asarray([0.0, pan_link, 0.0])
 
         quat_pan_to_base = habUtils.quat_from_angle_axis(
-            -1 * self.pan, np.asarray([0.0, 1.0, 0.0])
+            -1 * pan, np.asarray([0.0, 1.0, 0.0])
         )
 
         sensor_offset_base = habUtils.quat_rotate_vector(
@@ -226,8 +284,8 @@ def set_pan_tilt(self, pan, tilt, wait=True):
         self.pan = pan
         self.tilt = tilt
         sensor_offset, quat_base_to_sensor = self._compute_relative_pose(pan, tilt)
-        cur_state = self.agent.get_state()
-        sensor_position = sensor_offset + cur_state.position
+        cur_state = self.agent.get_state() # Habitat frame
+        sensor_position = cur_state.position + sensor_offset
         sensor_quat = cur_state.rotation * quat_base_to_sensor
         cur_state.sensor_states["rgb"].position = sensor_position
         cur_state.sensor_states["rgb"].rotation = sensor_quat

src/pyrobot/habitat/simulator.py

@@ -26,7 +26,7 @@ def __init__(
         print(self.sim_config)
 
     def get_agents():
-        """ Return a list of anget objects"""
+        """ Return a list of agent objects"""
         return self.sim.agents
 
     def get_sensors():

src/pyrobot/utils/util.py

@@ -187,7 +187,7 @@ def pix_to_3dpt(depth_im, rs, cs, intrinsic_mat, depth_map_factor, reduce=None,
     if isinstance(cs, np.ndarray):
         cs = cs.flatten()
     R, C = depth_im.shape
-    if reduce == "none":
+    if reduce == "none" or reduce is None:
         depth_im = depth_im[rs, cs]
     elif reduce == "mean":
         depth_im = np.array(