def get_nrmls(pcd, camera_location=(800, -200, 1800), toggledebug=False):
pcd_o3d = o3d_helper.nparray2o3dpcd(pcd)
pcd_o3d.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(
radius=20, max_nn=1000))
# for n in np.asarray(pcd.normals)[:10]:
# print(n)
# print("----------------")
o3d.geometry.PointCloud.orient_normals_towards_camera_location(
pcd_o3d, camera_location=camera_location)
# for n in np.asarray(pcd.normals)[:10]:
# print(n)
pcd_nrmls = np.asarray(pcd_o3d.normals)
pcd_nrmls = np.asarray(
[-n if np.dot(n, np.asarray([0, 0, 1])) < 0 else n for n in pcd_nrmls])
if toggledebug:
for i, n in enumerate(pcd_nrmls):
import random
v = random.choice(range(0, 100))
if v == 1:
base.pggen.plotArrow(base.render,
spos=pcd[i],
epos=pcd[i] + 10 * n)
base.run()
return pcd_nrmls
def get_objpcd(objcm, objmat4=np.eye(4), sample_num=100000, toggledebug=False):
objpcd = np.asarray(
objcm.sample_surface(nsample=sample_num, toggle_option=None))
objpcd = trans_pcd(objpcd, objmat4)
if toggledebug:
objpcd = o3d_helper.nparray2o3dpcd(copy.deepcopy(objpcd))
objpcd.paint_uniform_color([1, 0.706, 0])
o3d.visualization.draw_geometries([objpcd])
return objpcd
def get_objpcd_partial(objcm,
objmat4=np.eye(4),
sample_num=100000,
toggledebug=False):
objpcd = np.asarray(ts.sample_surface(objcm.trimesh, count=sample_num))
objpcd = trans_pcd(objpcd, objmat4)
grid = {}
for p in objpcd:
x = round(p[0], 0)
y = round(p[1], 0)
if str((x, y)) in grid.keys():
grid[str((x, y))].append(p)
else:
grid[str((x, y))] = [p]
objpcd_new = []
for k, v in grid.items():
z_max = max(np.array(v)[:, 2])
for p in v:
objpcd_new.append([p[0], p[1], z_max])
objpcd_new = np.array(objpcd_new)
print("Length of org pcd", len(objpcd))
print("Length of partial pcd", len(objpcd_new))
if toggledebug:
objpcd = o3d_helper.nparray2o3dpcd(copy.deepcopy(objpcd))
objpcd.paint_uniform_color([1, 0.706, 0])
o3d.visualization.draw_geometries([objpcd])
objpcd_partial = o3d_helper.nparray2o3dpcd(copy.deepcopy(objpcd_new))
objpcd_partial.paint_uniform_color([0, 0.706, 1])
o3d.visualization.draw_geometries([objpcd_partial])
# pcddnp = base.pg.genpointcloudnp(objpcd)
# pcddnp.reparentTo(base.render)
return objpcd_new
def get_plane(pcd, dist_threshold=0.2, toggledebug=False):
pcd_o3d = o3d_helper.nparray2o3dpcd(pcd)
plane, inliers = pcd_o3d.segment_plane(distance_threshold=dist_threshold,
ransac_n=3,
num_iterations=1000)
plane_pcd = pcd[inliers]
center = get_pcd_center(plane_pcd)
if toggledebug:
show_pcd(pcd[inliers], rgba=(1, 0, 0, 1))
base.pggen.plotArrow(base.render,
spos=center,
epos=plane[:3] * plane[3],
rgba=(1, 0, 0, 1))
return plane[:3], plane[3]
def get_objpcd_partial_bycampos(objcm,
objmat4=np.eye(4),
sample_num=100000,
cam_pos=np.array([860, 80, 1780]),
toggledebug=False):
def __get_angle(x, y):
lx = np.sqrt(x.dot(x))
ly = np.sqrt(y.dot(y))
cos_angle = x.dot(y) / (lx * ly)
angle = np.arccos(cos_angle)
return angle * 360 / 2 / np.pi
# return cos_angle
def sigmoid(angle):
return 1 / (1 + np.exp((angle - 90) / 90)) - 0.5
objpcd = np.asarray(ts.sample_surface(objcm.trimesh, count=sample_num))
objpcd_center = get_pcd_center(objpcd)
face_num = len(objcm.trimesh.face_normals)
objpcd_new = []
area_list = objcm.trimesh.area_faces
area_sum = sum(area_list)
for i, n in enumerate(objcm.trimesh.face_normals):
n = np.dot(n, objmat4[:3, :3])
angle = __get_angle(n, np.array(cam_pos - objpcd_center))
if angle > 90:
continue
else:
objcm_temp = copy.deepcopy(objcm)
# print(i, angle, sigmoid(angle))
mask_temp = [False] * face_num
mask_temp[i] = True
objcm_temp.trimesh.update_faces(mask_temp)
objpcd_new.extend(
np.asarray(
objcm_temp.sample_surface(
nsample=int(sample_num / area_sum * area_list[i] *
sigmoid(angle) * 100),
toggle_option=None)))
if len(objpcd_new) > sample_num:
objpcd_new = random.sample(objpcd_new, sample_num)
objpcd_new = np.array(objpcd_new)
objpcd_new = trans_pcd(objpcd_new, objmat4)
# print("Length of org pcd", len(objpcd))
# print("Length of source pcd", len(objpcd_new))
if toggledebug:
objpcd = o3d_helper.nparray2o3dpcd(copy.deepcopy(objpcd))
objpcd.paint_uniform_color([1, 0.706, 0])
o3d.visualization.draw_geometries([objpcd])
objpcd_partial = o3d_helper.nparray2o3dpcd(copy.deepcopy(objpcd_new))
objpcd_partial.paint_uniform_color([0, 0.706, 1])
o3d.visualization.draw_geometries([objpcd_partial])
# pcddnp = base.pg.genpointcloudnp(objpcd)
# pcddnp.reparentTo(base.render)
return objpcd_new
spos=center,
epos=plane[:3] * plane[3],
rgba=(1, 0, 0, 1))
return plane[:3], plane[3]
if __name__ == '__main__':
'''
set up env and param
'''
base, env = el.loadEnv_wrs()
objcm = el.loadObj("pentip.stl")
# source_pcd = np.asarray(ts.sample_surface(objcm.trimesh, count=10000))
source = o3d_helper.nparray2o3dpcd(source_pcd[source_pcd[:, 2] > 5])
# source.paint_uniform_color([0, 0.706, 1])
# o3d.visualization.draw_geometries([source])
# get_objpcd_partial(objcm, sample_num=10000, toggledebug=True)
# inithomomat = pickle.load(
# open(el.root + "/graspplanner/graspmap/pentip_cover_objmat4_list.pkl", "rb"))[1070]
#
# get_normals(get_objpcd(objcm, sample_num=10000))
# pcd, pcd_normals = get_objpcd_withnormals(objcm, sample_num=100000)
# for i, p in enumerate(pcd):
# base.pggen.plotArrow(base.render, spos=p, epos=p + 10 * pcd_normals[i])
# base.run()
get_objpcd_partial_bycampos(objcm, sample_num=10000, toggledebug=True)