robots/LoCoBot/locobot_navigation/orb_slam2_ros/scripts/orb_slam2_ros/pcdlib.py [77:152]:
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
    def get_pcd_ic(self, depth_im, rgb_im=None):
        """
        Returns the point cloud in camera's coordinate frame

        :param depth_im: depth image (shape: :math:`[H, W]`)
        :param rgb_im: rgb image (shape: :math:`[H, W, 3]`)

        :type depth_im: numpy.ndarray
        :type rgb_im: numpy.ndarray

        :returns: tuple (pts_in_cam, rgb_im)
                  pts_in_cam: point coordinates in
                              camera frame (shape: :math:`[4, N]`)
                  rgb: rgb values for pts_in_cam (shape: :math:`[N, 3]`)
        :rtype (numpy.ndarray, numpy.ndarray)
        """
        # pcd in camera from depth
        depth_im = depth_im[0 :: self.subsample_pixs, 0 :: self.subsample_pixs]
        rgb_im = rgb_im[0 :: self.subsample_pixs, 0 :: self.subsample_pixs]
        depth = depth_im.reshape(-1) / float(self.cfg_data["DepthMapFactor"])
        rgb = None
        if rgb_im is not None:
            rgb = rgb_im.reshape(-1, 3)
        if self.depth_threshold is not None:
            valid = depth > self.depth_threshold[0]
            if len(self.depth_threshold) > 1:
                valid = np.logical_and(valid, depth < self.depth_threshold[1])
            uv_one_in_cam = self.uv_one_in_cam[:, valid]
            depth = depth[valid]
            rgb = rgb[valid]
        else:
            uv_one_in_cam = self.uv_one_in_cam
        pts_in_cam = np.multiply(uv_one_in_cam, depth)
        pts_in_cam = np.concatenate(
            (pts_in_cam, np.ones((1, pts_in_cam.shape[1]))), axis=0
        )
        return pts_in_cam, rgb

    def get_pcd_iw(self, pts_in_cam, extrinsic_mat):
        """
        Returns the point cloud in the world coordinate frame

        :param pts_in_cam: point coordinates in
               camera frame (shape: :math:`[4, N]`)
        :param extrinsic_mat: extrinsic matrix for
               the camera (shape: :math:`[4, 4]`)

        :type pts_in_cam: numpy.ndarray
        :type extrinsic_mat: numpy.ndarray

        :return: point coordinates in
                 ORB-SLAM2's world frame (shape: :math:`[N, 3]`)
        :rtype: numpy.ndarray
        """
        # pcd in world
        pts_in_world = np.dot(extrinsic_mat, pts_in_cam)
        pts_in_world = pts_in_world[:3, :].T
        return pts_in_world

    def read_cfg(self, cfg_filename):
        """
        Reads the configuration file

        :param cfg_filename: configuration file name for ORB-SLAM2

        :type cfg_filename: string

        :return: configurations in the configuration file
        :rtype: dict
        """
        rospack = rospkg.RosPack()
        slam_pkg_path = rospack.get_path("orb_slam2_ros")
        cfg_path = os.path.join(slam_pkg_path, "cfg", cfg_filename)
        with open(cfg_path, "r") as f:
            for i in range(1):
                f.readline()
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -



src/pyrobot/locobot/camera.py [418:497]:
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
    def get_pcd_ic(self, depth_im, rgb_im=None):
        """
        Returns the point cloud (filtered by minimum
        and maximum depth threshold)
        in camera's coordinate frame

        :param depth_im: depth image (shape: :math:`[H, W]`)
        :param rgb_im: rgb image (shape: :math:`[H, W, 3]`)

        :type depth_im: np.ndarray
        :type rgb_im: np.ndarray

        :returns: tuple (pts_in_cam, rgb_im)

                  pts_in_cam: point coordinates in
                              camera frame (shape: :math:`[4, N]`)

                  rgb: rgb values for pts_in_cam (shape: :math:`[N, 3]`)
        :rtype tuple(np.ndarray, np.ndarray)
        """
        # pcd in camera from depth
        depth_im = depth_im[0 :: self.subsample_pixs, 0 :: self.subsample_pixs]
        rgb_im = rgb_im[0 :: self.subsample_pixs, 0 :: self.subsample_pixs]
        depth = depth_im.reshape(-1) / float(self.cfg_data["DepthMapFactor"])
        rgb = None
        if rgb_im is not None:
            rgb = rgb_im.reshape(-1, 3)
        if self.depth_threshold is not None:
            valid = depth > self.depth_threshold[0]
            if len(self.depth_threshold) > 1:
                valid = np.logical_and(valid, depth < self.depth_threshold[1])
            uv_one_in_cam = self.uv_one_in_cam[:, valid]
            depth = depth[valid]
            rgb = rgb[valid]
        else:
            uv_one_in_cam = self.uv_one_in_cam
        pts_in_cam = np.multiply(uv_one_in_cam, depth)
        pts_in_cam = np.concatenate(
            (pts_in_cam, np.ones((1, pts_in_cam.shape[1]))), axis=0
        )
        return pts_in_cam, rgb

    def get_pcd_iw(self, pts_in_cam, extrinsic_mat):
        """
        Returns the point cloud in the world coordinate frame

        :param pts_in_cam: point coordinates in
               camera frame (shape: :math:`[4, N]`)
        :param extrinsic_mat: extrinsic matrix for
               the camera (shape: :math:`[4, 4]`)

        :type pts_in_cam: np.ndarray
        :type extrinsic_mat: np.ndarray

        :return: point coordinates in
                 ORB-SLAM2's world frame (shape: :math:`[N, 3]`)
        :rtype: np.ndarray
        """
        # pcd in world
        pts_in_world = np.dot(extrinsic_mat, pts_in_cam)
        pts_in_world = pts_in_world[:3, :].T
        return pts_in_world

    def read_cfg(self, cfg_filename):
        """
        Reads the configuration file

        :param cfg_filename: configuration file name for ORB-SLAM2

        :type cfg_filename: string

        :return: configurations in the configuration file
        :rtype: dict
        """
        rospack = rospkg.RosPack()
        slam_pkg_path = rospack.get_path("orb_slam2_ros")
        cfg_path = os.path.join(slam_pkg_path, "cfg", cfg_filename)
        with open(cfg_path, "r") as f:
            for i in range(1):
                f.readline()
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -