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 registration_pegboard(self, img_color, img_point):
# color masking
img_hsv = cv2.cvtColor(img_color, cv2.COLOR_BGR2HSV)
img_color_masked = cv2.inRange(img_hsv, self.lower_red, self.upper_red)
# get points inside contour
cnts, _ = cv2.findContours(img_color_masked, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = sorted(cnts, key=cv2.contourArea, reverse=True)
peg_board = np.zeros_like(img_color)
cv2.drawContours(peg_board, cnts, 0, (255, 255, 255), -1)
pnt_board = img_point[np.all(peg_board == [255, 255, 255], axis=-1)]
pnt_board = BlockDetection3D.remove_nan(pnt_board) * 0.001 # (m)
pcl_board = o3d.geometry.PointCloud()
pcl_board.points = o3d.utility.Vector3dVector(pnt_board)
# load target (pegboard model)
pcl_model = o3d.io.read_point_cloud(root + 'img/peg_board.pcd')
pnt_model = PCLRegistration.convert(pcl_model)[1] * 0.001 # to (m)
pcl_model = PCLRegistration.convert(pnt_model)[0]
print(pnt_board)
print(pnt_model)
# registration
Tpc = PCLRegistration.registration(source=pcl_board, target=pcl_model)
PCLRegistration.display_registration(pcl_board, pcl_model, Tpc)
print('Tpc= ', Tpc)
np.save('Tpc.npy', Tpc)
print('Tpc saved')
def prune_block(self, pnt_blk, visualize=False):
if pnt_blk == []:
pruned = []
else:
plane, outlier, coefficient = PCLRegistration.segment_surface(
pnt_blk, dist_threshold=0.5, visualize=visualize)
labels = np.array(
plane.cluster_dbscan(eps=1,
min_points=30,
print_progress=False))
clustered = np.asarray(plane.points)
arg = np.histogram(labels, [-1, 0, 1, 2, 3, 4, 5])[0].argmax()
pruned = clustered[labels == arg - 1]
return pruned
def prune_block_servo(self, pnt_blk, visualize=False):
pruned = o3d.geometry.PointCloud()
for i in range(2):
plane, outlier = PCLRegistration.segment_surface(
pnt_blk, dist_threshold=0.5, visualize=visualize)
if len(plane.points) > 500:
labels = np.array(
plane.cluster_dbscan(eps=1,
min_points=40,
print_progress=False))
clustered = np.asarray(plane.points)
arg = np.histogram(labels, [-1, 0, 1, 2, 3, 4, 5])[0].argmax()
pruned = pruned + clustered[labels == arg - 1]
pnt_blk = outlier
return pruned
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
]