def main():
# // Load input file into a PointCloud<T> with an appropriate type
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
# // Load bun0.pcd -- should be available with the PCL archive in test
# pcl::io::loadPCDFile ("bun0.pcd", *cloud);
points = np.load("sample.npy").transpose(1, 0)
filtered_neighbor_list, coordinate = MLS(points)
cloud = pcl.load('pcd/origin.pcd')
print('cloud(size) = ' + str(cloud.size))
# // Create a KD-Tree
# pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
start = time()
tree = cloud.make_kdtree()
# tree = cloud.make_kdtree_flann()
# blankCloud = pcl.PointCloud()
# tree = blankCloud.make_kdtree()
# // Output has the PointNormal type in order to store the normals calculated by MLS
# pcl::PointCloud<pcl::PointNormal> mls_points;
# mls_points = pcl.PointCloudNormal()
# // Init object (second point type is for the normals, even if unused)
# pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls;
# mls.setComputeNormals (true);
#
# // Set parameters
# mls.setInputCloud (cloud);
# mls.setPolynomialFit (true);
# mls.setSearchMethod (tree);
# mls.setSearchRadius (0.03);
#
# // Reconstruct
# mls.process (mls_points);
mls = cloud.make_moving_least_squares()
# print('make_moving_least_squares')
mls.set_Compute_Normals(True)
mls.set_polynomial_fit(True)
mls.set_polynomial_order(3)
mls.set_Search_Method(tree)
mls.set_search_radius(0.1)
print('set parameters')
mls_points = mls.process()
projected = np.asarray(mls_points)
points2pcd(projected, "projected_mls")
print("standard time is: ", time() - start)
# Save output
# pcl::io::savePCDFile ("bun0-mls.pcd", mls_points);
pcl.save_PointNormal(mls_points, 'bun0-mls.pcd')
def main():
# // Load input file into a PointCloud<T> with an appropriate type
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
# // Load bun0.pcd -- should be available with the PCL archive in test
# pcl::io::loadPCDFile ("bun0.pcd", *cloud);
cloud = pcl.load('./examples/official/Surface/bun0.pcd')
print('cloud(size) = ' + str(cloud.size))
# // Create a KD-Tree
# pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree = cloud.make_kdtree()
# tree = cloud.make_kdtree_flann()
# blankCloud = pcl.PointCloud()
# tree = blankCloud.make_kdtree()
# // Output has the PointNormal type in order to store the normals calculated by MLS
# pcl::PointCloud<pcl::PointNormal> mls_points;
# mls_points = pcl.PointCloudNormal()
# // Init object (second point type is for the normals, even if unused)
# pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls;
# mls.setComputeNormals (true);
#
# // Set parameters
# mls.setInputCloud (cloud);
# mls.setPolynomialFit (true);
# mls.setSearchMethod (tree);
# mls.setSearchRadius (0.03);
#
# // Reconstruct
# mls.process (mls_points);
mls = cloud.make_moving_least_squares()
# print('make_moving_least_squares')
mls.set_Compute_Normals(True)
mls.set_polynomial_fit(True)
mls.set_Search_Method(tree)
mls.set_search_radius(0.03)
print('set parameters')
mls_points = mls.process()
# Save output
# pcl::io::savePCDFile ("bun0-mls.pcd", mls_points);
pcl.save_PointNormal(mls_points, 'bun0-mls.pcd')
# blankCloud = pcl.PointCloud()
# tree = blankCloud.make_kdtree()
# // Output has the PointNormal type in order to store the normals calculated by MLS
# pcl::PointCloud<pcl::PointNormal> mls_points;
# mls_points = pcl.PointCloudNormal()
# // Init object (second point type is for the normals, even if unused)
# pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls;
# mls.setComputeNormals (true);
#
# // Set parameters
# mls.setInputCloud (cloud);
# mls.setPolynomialFit (true);
# mls.setSearchMethod (tree);
# mls.setSearchRadius (0.03);
#
# // Reconstruct
# mls.process (mls_points);
mls = cloud.make_moving_least_squares()
# print('make_moving_least_squares')
mls.set_Compute_Normals (True)
mls.set_polynomial_fit (True)
mls.set_Search_Method (tree)
mls.set_search_radius (0.03)
print('set parameters')
mls_points = mls.process ()
# Save output
# pcl::io::savePCDFile ("bun0-mls.pcd", mls_points);
pcl.save_PointNormal(mls_points, 'bun0-mls.pcd')
def main():
# File i/o -- Folder name
if (len(sys.argv) > 1):
directory = sys.argv[1]
else:
print("Need a dirName argument... Exiting")
exit()
n_clouds = len([
len(r) for r in os.listdir(directory)
if os.path.isfile(os.path.join(directory, r))
])
# file handling
if not os.path.exists("{}/smooth".format(directory)):
print("Made Directory: {}/smooth".format(directory))
os.mkdir("{}/smooth".format(directory))
else:
shutil.rmtree("{}/smooth".format(directory))
os.makedirs("{}/smooth".format(directory))
print("Re-made Directory: {}/smooth".format(directory))
print("Clouds to process: {}\n".format(n_clouds))
# process each cloud
for x in range(n_clouds):
f_name = "{}/inliers/{}.pcd".format(directory, x + 1)
cloud = pcl.load(f_name)
print('cloud(size) = ' + str(cloud.size))
tree = cloud.make_kdtree()
mls = cloud.make_moving_least_squares()
mls.set_Compute_Normals(True)
mls.set_polynomial_fit(True)
mls.set_Search_Method(tree)
mls.set_search_radius(0.03)
print('set parameters & processing...')
mls_points = mls.process()
print('cloud(size) = ' + str(mls_points.size))
f_name2 = "{}/smooth/{}.pcd".format(directory, x + 1)
pcl.save_PointNormal(mls_points, f_name2)
print("Processed and saved!\n")
# display for last one
if (x + 1 == n_clouds):
normals = pcl.load(f_name2)
viewer = pcl.pcl_visualization.PCLVisualizering('CLOUD')
viewer2 = pcl.pcl_visualization.PCLVisualizering('RESAMPLED')
pccolor = pcl.pcl_visualization.PointCloudColorHandleringCustom(
cloud, 255, 255, 255)
kpcolor = pcl.pcl_visualization.PointCloudColorHandleringCustom(
normals, 255, 255, 255)
viewer.AddPointCloud_ColorHandler(cloud, pccolor)
viewer2.AddPointCloud_ColorHandler(normals, kpcolor, b'normals')
v = True
while v:
v = not (viewer.WasStopped())
viewer.SpinOnce()
v2 = not (viewer.WasStopped())
viewer2.SpinOnce()
def cal_grasp(msg, cam_pos_):
"""
抓取姿态生成
:param msg: ROS点云消息
:param cam_pos_: 摄像头姿态
:return:
"""
points_ = pointclouds.pointcloud2_to_xyz_array(msg)
points_ = points_.astype(np.float32)
remove_white = False
if remove_white:
points_ = remove_white_pixel(msg, points_, vis=True)
# begin voxel points
n = n_voxel # parameter related to voxel method
# gpg improvements, highlights: flexible n parameter for voxelizing.
points_[:, 0] = points_[:, 0] + 0.025 # liang: as the kinect2 is not well calibrated, here is a work around
points_[:, 2] = points_[:, 2] # + 0.018 # liang: as the kinect2 is not well calibrated, here is a work around
points_voxel_ = get_voxel_fun(points_, n) # point cloud down sample
# 点数过少, 调整降采样参数
if len(points_) < 2000: # should be a parameter
while len(points_voxel_) < len(points_)-15:
points_voxel_ = get_voxel_fun(points_, n)
n = n + 100
rospy.loginfo("the voxel has {} points, we want get {} points".format(len(points_voxel_), len(points_)))
rospy.loginfo("the voxel has {} points, we want get {} points".format(len(points_voxel_), len(points_)))
points_ = points_voxel_ # 降采样之后的点云
remove_points = False
if remove_points:
points_ = remove_table_points(points_, vis=True)
point_cloud = pcl.PointCloud(points_) # 传入pcl处理
# 计算表面法线
norm = point_cloud.make_NormalEstimation()
pcl.save_PointNormal()
norm.set_KSearch(30) # critical parameter when calculating the norms
normals = norm.compute()
surface_normal = normals.to_array()
surface_normal = surface_normal[:, 0:3]
# 处理法线方向
vector_p2cam = cam_pos_ - points_
vector_p2cam = vector_p2cam / np.linalg.norm(vector_p2cam, axis=1).reshape(-1, 1)
tmp = np.dot(vector_p2cam, surface_normal.T).diagonal()
angel = np.arccos(np.clip(tmp, -1.0, 1.0))
wrong_dir_norm = np.where(angel > np.pi * 0.5)[0]
tmp = np.ones([len(angel), 3])
tmp[wrong_dir_norm, :] = -1
surface_normal = surface_normal * tmp
select_point_above_table = 0.010
# modify of gpg: make it as a parameter. avoid select points near the table.
points_for_sample = points_[np.where(points_[:, 2] > select_point_above_table)[0]]
if len(points_for_sample) == 0:
rospy.loginfo("Can not select point, maybe the point cloud is too low?")
return [], points_, surface_normal
yaml_config['metrics']['robust_ferrari_canny']['friction_coef'] = value_fc
if not using_mp:
rospy.loginfo("Begin cal grasps using single thread, slow!")
grasps_together_ = ags.sample_grasps(point_cloud, points_for_sample, surface_normal, num_grasps,
max_num_samples=max_num_samples, show_final_grasp=show_final_grasp)
else:
# begin parallel grasp:
rospy.loginfo("Begin cal grasps using parallel!")
def grasp_task(num_grasps_, ags_, queue_):
ret = ags_.sample_grasps(point_cloud, points_for_sample, surface_normal, num_grasps_,
max_num_samples=max_num_samples, show_final_grasp=show_final_grasp)
queue_.put(ret)
queue = mp.Queue()
num_grasps_p_worker = int(num_grasps/num_workers)
workers = [mp.Process(target=grasp_task, args=(num_grasps_p_worker, ags, queue)) for _ in range(num_workers)]
[i.start() for i in workers]
grasps_together_ = []
for i in range(num_workers):
grasps_together_ = grasps_together_ + queue.get()
rospy.loginfo("Finish mp processing!")
rospy.loginfo("Grasp sampler finish, generated {} grasps.".format(len(grasps_together_)))
return grasps_together_, points_, surface_normal