def compose_depth(self, depthmap_size):
'''
Create a depth map using points of point cloud.
Treat point cloud as list of triplets (i, j, d), where i (rounded to the closest int) - number of row,
j (rounded to the closest int) - number of column, d - depth stored in pixel (i,j).
For two points with coinciding i's and j's stores one with the smallest value of d (i.e. take the closest point).
Note: do not confuse with projection of pointcloud using certain camera.
'''
check_mtrx(np.array(depthmap_size), "depth map size", (2,))
depthmap = np.zeros(depthmap_size)
xyd_list = self.p3d.T
xyd_list[:2] = np.round(xyd_list[:2])
mask = (xyd_list[0] >= 0) & (xyd_list[0] < depthmap_size[1]) & (xyd_list[1] >= 0) & (xyd_list[1] < depthmap_size[0])
xyd_list = xyd_list[:, mask]
depthmap[np.uint16(xyd_list[1]), np.uint16(xyd_list[0])] = xyd_list[2]
return depthmap
def from_vec_vec(cl, rot_vec, t_vec):
rot_vec = np.array(rot_vec).astype(
RotoTranslate.rt_transform_default_dtype)
t_vec = np.array(t_vec).astype(
RotoTranslate.rt_transform_default_dtype)
check_mtrx(rot_vec,
what_mtrx='3d rotation vector',
required_shape=(3, ))
check_mtrx(t_vec,
what_mtrx='3d translation vector',
required_shape=(3, ))
return RotoTranslate.from_mat_vec(cv2.Rodrigues(rot_vec)[0], t_vec)
def __init__(self,
cam_mtrx_dist: np.ndarray,
cam_mtrx_undist: np.ndarray = None,
resolution=None,
dist_coeffs=None,
other_params: dict = None):
self._cam_mtrx_dist = np.array(cam_mtrx_dist)
check_mtrx(self._cam_mtrx_dist, 'intrinsic matrix')
self._cam_mtrx_undist = cam_mtrx_undist
if not self._cam_mtrx_undist is None:
self._cam_mtrx_undist = np.array(self._cam_mtrx_undist)
check_mtrx(self._cam_mtrx_undist,
'intrinsic undistorted matrix',
required_shape=(3, 3))
self._resolution = resolution
if not self._resolution is None:
self._resolution = np.array(self._resolution)
check_mtrx(self._resolution, 'resolution', required_shape=(2, ))
self._dist_coeffs = dist_coeffs
if not self._dist_coeffs is None:
self._dist_coeffs = np.array(self._dist_coeffs)
check_mtrx(self._dist_coeffs,
'distortion coefficients list',
required_shape=(8, ))
self._other_params = other_params
if not self._other_params is None:
if type(self._other_params) != dict:
raise ValueError("other_params should be a dict")
def __init__(self, rot_mtrx_4x4, t_vec_4d):
self._rot_mat = np.array(rot_mtrx_4x4).astype(
RotoTranslate.rt_transform_default_dtype)
self._t_vec = np.array(t_vec_4d).astype(
RotoTranslate.rt_transform_default_dtype)
check_mtrx(self._rot_mat,
what_mtrx='4x4 rotation matrix',
required_shape=(4, 4))
check_mtrx(self._t_vec,
what_mtrx='4d translation vector',
required_shape=(4, ))
self._inv_rot_mat = np.linalg.inv(self._rot_mat)
self._inv_t_vec = -self._inv_rot_mat @ self._t_vec
def __call__(self, depthmap):
check_mtrx(depthmap, "depthmap", self._intrinsics.resolution)
height, width = self._intrinsics.resolution
depthmap_flattened = depthmap.reshape((-1))
assert depthmap_flattened.shape[0] == len(self._undistortion_lut)
dst_img = np.zeros_like(depthmap_flattened)
mask = self._undistortion_lut[:,
0] != DepthDistortionRectifier.INVALID_LUT_DATA
if self._interpolation_type == 'nearest':
dst_img[mask] = depthmap_flattened[
self._undistortion_lut[mask:, 1] * width +
self._undistortion_lut[mask:, 0]]
elif self._interpolation_type == 'bilinear':
neighbors = np.zeros((depthmap_flattened.shape[0], 4))
neighbors[mask] = np.array([
depthmap_flattened[np.int32(self._undistortion_lut[mask, 1] *
width +
self._undistortion_lut[mask, 0])],
depthmap_flattened[np.int32(self._undistortion_lut[mask, 1] *
width +
self._undistortion_lut[mask, 0] +
1)],
depthmap_flattened[
np.int32((self._undistortion_lut[mask, 1] + 1) * width +
self._undistortion_lut[mask, 0])],
depthmap_flattened[
np.int32((self._undistortion_lut[mask, 1] + 1) * width +
self._undistortion_lut[mask, 0] + 1)]
]).T
idxs_where_eq_zero = np.unique(np.argwhere(neighbors == 0)[:, 0])
mask_where_not_eq_zero = np.ones(depthmap_flattened.shape[0],
dtype=bool)
mask_where_not_eq_zero[idxs_where_eq_zero] = False
mask_where_calc = mask_where_not_eq_zero
dst_img[mask_where_calc] = np.sum(
neighbors[mask_where_calc] *
self._undistortion_lut[mask_where_calc, 2:],
axis=1) + 0.5
return dst_img.reshape(depthmap.shape)
def from_mat_vec(cl, rot_mtrx, t_vec):
rot_mtrx = np.array(rot_mtrx).astype(
RotoTranslate.rt_transform_default_dtype)
t_vec = np.array(t_vec).astype(
RotoTranslate.rt_transform_default_dtype)
check_mtrx(rot_mtrx,
what_mtrx='3x3 rotation matrix',
required_shape=(3, 3))
check_mtrx(t_vec,
what_mtrx='3d translation vector',
required_shape=(3, ))
rot_mat_4x4 = np.zeros((4, 4))
rot_mat_4x4[:3, :3] = rot_mtrx
rot_mat_4x4[3, 3] = 1
t_vec_4d = np.hstack((t_vec, [0]))
return RotoTranslate(rot_mat_4x4, t_vec_4d)