def find_block_servo_handover(self, img_color, img_point):
# color masking & transform points to pegboard coordinate
pnt_masked = self.mask_color(img_color, img_point,
[self.lower_red, self.upper_red])
pnt_masked = self.remove_nan(pnt_masked)
pnt_transformed = U.transform(pnt_masked * 0.001, self.Tpc) * 1000
# block image masking by depth
pnt_blk = self.mask_depth(pnt_transformed, self.depth_block_servo)
# remove outliers
# pnt_blk, _ = PCLRegistration.remove_outliers(pnt_blk, nb_points=20, radius=2, visualize=False)
pcl_blk, pnt_blk = PCLRegistration.convert(pnt_blk)
# registration
if len(pnt_blk) < 100:
pose_blk = []
pnt_mask = []
else:
T = PCLRegistration.registration(pcl_blk,
self.pcl_model_servo,
downsample=2,
use_svr=False,
save_image=False,
visualize=False)
T = np.linalg.inv(T) # transform from model to block
blk_ang = self.get_pose(T)
pose_blk = [blk_ang, T]
pnt_mask = T[:3, :3].dot(
self.pnt_model_servo.T).T + T[:3,
-1].T # transformed mask points
return pose_blk, pnt_blk, pnt_mask
def find_pegs(self, img_color, img_point, visualize=False):
# color masking & transform points to pegboard coordinate
pnt_masked = self.mask_color(img_color, img_point,
[self.lower_red, self.upper_red])
pnt_masked = self.remove_nan(pnt_masked)
pnt_transformed = U.transform(pnt_masked * 0.001, self.Tpc) * 1000
# depth masking
pnt_pegs = self.mask_depth(pnt_transformed, self.depth_peg)
clustered = self.cluster_pegs(pnt_pegs,
density=3,
min_size=30,
visualize=visualize)
self.pnt_pegs = BlockDetection2D.sort_pegs(
clustered) # [[x0, y0, z0], ..., [xn, yn, zn]]
def find_block(self, block_number, img_color, img_point, which_arm=None):
# color masking & transform points to pegboard coordinate
pnt_masked = self.mask_color(img_color, img_point,
[self.lower_red, self.upper_red])
pnt_masked = self.remove_nan(pnt_masked)
pnt_transformed = U.transform(pnt_masked * 0.001, self.Tpc) * 1000
# block image masking by depth
pnt_blks = self.mask_depth(pnt_transformed, self.depth_block)
# cluster blocks by peg position
pnt_blk = self.cluster_block(pnt_blks, self.pnt_pegs[block_number],
self.d_cr_min, self.d_cr_max)
# prune block points by segmenting two planes
pnt_blk = self.prune_block(pnt_blk)
# registration
if pnt_blk == []:
pose_blk = []
pnt_mask = []
else:
pcl_blk = o3d.geometry.PointCloud()
pcl_blk.points = o3d.utility.Vector3dVector(pnt_blk)
st = time.time()
T = PCLRegistration.registration(pcl_blk,
self.pcl_model,
downsample=2,
use_svr=False,
save_image=False,
visualize=False)
print(time.time() - st)
T = np.linalg.inv(T) # transform from model to block
blk_ang = self.get_pose(T)
pose_blk = [blk_ang, T]
pnt_mask = T[:3, :3].dot(
self.pnt_model.T).T + T[:3, -1].T # transformed mask points
return pose_blk, pnt_blk, pnt_mask
def find_block_all(self, img_color, img_point):
# color masking & transform points to pegboard coordinate
pnt_masked = self.mask_color(img_color, img_point,
[self.lower_red, self.upper_red])
pnt_masked = self.remove_nan(pnt_masked)
pnt_transformed = U.transform(pnt_masked * 0.001, self.Tpc) * 1000
# block image masking by depth
pnt_blks = self.mask_depth(pnt_transformed, self.depth_block)
# cluster blocks by peg position
pnt_blks = self.cluster_blocks(pnt_blks, self.pnt_pegs, self.d_cr_min,
self.d_cr_max)
# prune block points by segmenting two planes
self.pnt_blks = self.prune_blocks(pnt_blks)
# registration
Ts = []
for b in self.pnt_blks:
if b == []:
Ts.append([])
else:
pcl_blks = o3d.geometry.PointCloud()
pcl_blks.points = o3d.utility.Vector3dVector(b)
T = PCLRegistration.registration(pcl_blks,
self.pcl_model,
use_svr=False,
save_image=False,
visualize=False)
Ts.append(np.linalg.inv(T))
self.pose_blks = self.get_poses(Ts) # [n, angle, T]
self.pnt_masks = [
T[:3, :3].dot(self.pnt_model.T).T + T[:3, -1].T if T != [] else []
for T in Ts
]