import argparse
parser = argparse.ArgumentParser(
description='PointCloudLibrary example: Remove outliers')
parser.add_argument('--Removal',
'-r',
choices=('Radius', 'Condition'),
default='',
help='RadiusOutlier/Condition Removal')
args = parser.parse_args()
# int main (int argc, char** argv)
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
cloud = pcl.PointCloud()
cloud_filtered = pcl.PointCloud()
# // Fill in the cloud data
# cloud->width = 5;
# cloud->height = 1;
# cloud->points.resize (cloud->width * cloud->height);
#
# for (size_t i = 0; i < cloud->points.size (); ++i)
# {
# cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
# cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
# cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
# }
#
# x,y,z
def __init__(self, args, root_folder="./NaverML_indoor"):
self.pose_ld = args.pose_ld
self.pose_covisibility = args.pose_covisibility
self.pose_pointcloud_load = args.pose_pointcloud_load
self.dataset = args.dataset
self.pose_timechecker = args.pose_timechecker
self.pose_noniter = args.pose_noniter
self.pose_cupy = args.pose_cupy
if self.pose_ld == 0: # SIFT
self.local_descriptor = he.SIFT(root=False)
elif self.pose_ld == 1: # rootSIFT
self.local_descriptor = he.SIFT(root=True)
elif self.pose_ld == 2: # D2 SS
self.local_descriptor = ex.D2Net_local_extractor(args=args,
model_file="./arxiv/d2_tf.pth",
use_relu=True,
max_edge=1600,
max_sum_edges=2800,
preprocessing='caffe',
multiscale=False)
elif self.pose_ld == 3: # D2 MS
self.local_descriptor = ex.D2Net_local_extractor(args=args,
model_file="./arxiv/d2_tf.pth",
use_relu=True,
max_edge=1600,
max_sum_edges=2800,
preprocessing='caffe',
multiscale=True)
elif self.pose_ld == 4: # SuperGlue
superPointdict=dict()
superPointdict["nms_radius"]=4
superPointdict["keypoint_threshold"]= 0.005
superPointdict["max_keypoints"]=1024
self.local_descriptor = ex.SuperPoint(superPointdict).eval().cuda()
if self.dataset==0:
self.cld_path = "img2pcds/indoor_b1"
elif self.dataset==1:
self.cld_path = "img2pcds/indoor_1f"
if self.pose_pointcloud_load is False:
if self.dataset==0:
self.cloud = pcl.load(os.path.join(root_folder, "b1", "train", "PointCloud_all", "map.pcd"))
self.cld_arr = self.cloud.to_array()
mask = (self.cld_arr[:,2]>-5.8) & (self.cld_arr[:,2]<0)
self.cld_arr = self.cld_arr[mask]
self.cloud = pcl.PointCloud()
self.cloud.from_array(self.cld_arr)
resolution = 0.2
self.octree = self.cloud.make_octreeSearch(resolution)
self.octree.add_points_from_input_cloud()
elif self.dataset==1:
self.cloud = pcl.load(os.path.join(root_folder, "1f", "train", "PointCloud_all", "map.pcd"))
self.cld_arr = self.cloud.to_array()
mask = (self.cld_arr[:,2]>0)
self.cld_arr = self.cld_arr[mask]
self.cloud = pcl.PointCloud()
self.cloud.from_array(self.cld_arr)
resolution = 0.2
self.octree = self.cloud.make_octreeSearch(resolution)
self.octree.add_points_from_input_cloud()
else:
raise ValueError("check PoseEstimation params")
self.grayscele_fn = db.Grayscale()
'/nfs/diskstation/projects/dex-net/placing/datasets/real/sample_ims_05_22/depth_ims_numpy/image_000001.npy'
)
# Create a DepthImage using the array
ci = CameraIntrinsics.load(
'/nfs/diskstation/projects/dex-net/placing/datasets/real/sample_ims_05_22/camera_intrinsics.intr'
)
di = DepthImage(image, frame=ci.frame)
pc = ci.deproject(di)
## Visualize the depth image
#vis2d.figure()
#vis2d.imshow(di)
#vis2d.show()
# Make and display a PCL type point cloud from the image
p = pcl.PointCloud(pc.data.T.astype(np.float32))
# Make a segmenter and segment the point cloud.
seg = p.make_segmenter()
seg.set_model_type(pcl.SACMODEL_PARALLEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
seg.set_distance_threshold(0.005)
indices, model = seg.segment()
print(model)
#pdb.set_trace()
vis3d.figure()
pc_plane = pc.data.T[indices]
pc_plane = pc_plane[np.where(pc_plane[::, 1] < 0.16)]
maxes = np.max(pc_plane, axis=0)
def setUp(self):
self.a = np.array(np.mat(SEGDATA, dtype=np.float32))
self.p = pcl.PointCloud()
self.p.from_array(self.a)
self.segment = self.p.make_segmenter()
def main():
baxter_test = Baxter_Test()
rate = rospy.Rate(baxter_test._rate)
#retrieve point cloud data from Baxter
point_cloud_msg = ros_topic_get("/camera/depth_registered/points", PointCloud2)
point_cloud_array = pointclouds.pointcloud2_to_xyz_array(point_cloud_msg)
point_cloud_array = transform_matrix(point_cloud_array, "/camera_rgb_optical_frame", "/base")
point_cloud = pcl.PointCloud() #create new pcl PointCloud
point_cloud.from_list(point_cloud_array) #import data to PointCloud from 2d matrix
#Experimental code for the hand mounted camera
print "looking up hand camera"
"""f200_cloud_msg = ros_topic_get("/camera/depth/points", PointCloud2)
print f200_cloud_msg
point_cloud_array = pointclouds.pointcloud2_to_xyz_array(f200_cloud_msg)
print point_cloud_array"""
#filter data
fil = point_cloud.make_passthrough_filter()
fil.set_filter_field_name("x")
fil.set_filter_limits(0.5, 1)
cloud_filtered_x = fil.filter()
fil = cloud_filtered_x.make_passthrough_filter()
fil.set_filter_field_name("z")
fil.set_filter_limits(-0.1, 0.5)
cloud_filtered = fil.filter()
#segment data
seg = cloud_filtered.make_segmenter()
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
seg.set_distance_threshold(0.03)
indices, model = seg.segment()
#remove table plane based on model
object_cloud = filter_plane(model, cloud_filtered, 0.1)
print object_cloud
X = object_cloud.to_array()
#our PCA only needs to deal with x-y plane, remove z values
X = X[:,[0,1]]
pca = PCA(n_components=2)
X = pca.fit_transform(X)
#calculate object centroid
object_centroid = get_centroid(object_cloud)
#calculate object bounding box
bounding_box = fit_bounding_box(object_cloud, pca.components_, object_centroid)
#baxter_test.move_left_to_coord(object_centroid)
publisher = rospy.Publisher("/baxter_test", MarkerArray, queue_size = 10)
marker_array = MarkerArray()
#PCA vector sanity check (if needed)
"""count = 0
for vector in pca.components_:
vector_marker = Marker()
vector_marker.header.frame_id = "/base"
vector_marker.header.stamp = rospy.Time(0)
vector_marker.type = vector_marker.ARROW
vector_marker.action = vector_marker.ADD
start = Point()
vector_marker.points = [createPoint(object_centroid[0], object_centroid[1], 0),
createPoint(object_centroid[0] + vector[0],
object_centroid[1] + vector[1], 0)]
if count == 0:
vector_marker.color.a = 1.0 #first vector green
vector_marker.color.r = 0.0
vector_marker.color.g = 1.0
vector_marker.color.b = 0.0
else:
vector_marker.color.a = 1.0 #second vector blue
vector_marker.color.r = 0.0
vector_marker.color.g = 0.0
vector_marker.color.b = 1.0
vector_marker.scale.x = 0.01
vector_marker.scale.y = 0.02
vector_marker.id = count
marker_array.markers.append(vector_marker)
count += 1"""
#continually do stuff
while not rospy.is_shutdown():
publisher.publish(marker_array)
#baxter_test.move_left_to_coord(object_centroid)
rate.sleep()
def run(self):
"""
This function holds the main loop that continuously finds walls and updates the wall distance
"""
r = rospy.Rate(
3
) #Attempts to run at a rate of 10 times a second (although never reaches this speed)
while not rospy.is_shutdown(): #Start main while loop
if not self.actualP is None and not self.position is None: #and not np.any(self.camera_corners) is None: #Wait until phone has found a pointcloud and a phone position
self.m.acquire() # LOCK
actualPCopy = self.pcloud_transform(self.actualP, 'odom')
if actualPCopy is None: #If something went wrong with the transform (if the transforms haven't been set yet or some timing issues happen)
pass #continue loop
else: #otherwise:
self.CurrP = self.CloudList(
actualPCopy
)[::self.
resolutionfactor] # squeeze out the third dimension and sample based on resolutionfactor.
cloud_filtered = pcl.PointCloud(
self.CurrP
) #Convert points into pcl Point Cloud (different from Ros point cloud)
seg = cloud_filtered.make_segmenter(
) #Set segmenter (tries to find segments in pointcloud)
seg.set_optimize_coefficients(True) #Not sure what this is
seg.set_model_type(
pcl.SACMODEL_PLANE
) #Set model that segmenter finds to a plane
seg.set_method_type(
pcl.SAC_RANSAC
) #Set method for finding plane to RANSAC algorithm
seg.set_distance_threshold(
self.planeDistThreshold) #Not sure what this is
match = False #The variable that determines if the previous plane is the same plane as found now.
try: #Start Try statment (a lot can go wrong in the following code from timing issues, to not having enough data to set a plane. In the end, the try except is just a much easier and probably quicker way to skip the current iteration and move to the next one)
for i in range(
self.planetrynum
): #Continue trying to find planes for self.planetrynum times.
if self.CurrP.shape[
0] < self.robustThreshold: #make sure the pointcloud has enough points to find a plane.
print('pointcloud too empty')
break
check = False #variable that determines if code should check if the current wall or previous wall is closer.
usesave = None #variable that determines whether or not to use the current wall or the previous wall.
indices, plane_model = seg.segment(
) #Receive plane_model [a, b, c, d] for equation (ax + by + cz + d = 0), also receive indices in the plane
if self.saved_plane_model is not None: #If there is a previous plane
if (
abs(plane_model[0] -
self.saved_plane_model[0]) <
self.abc_match_threshold
and abs(plane_model[1] -
self.saved_plane_model[1]) <
self.abc_match_threshold
and abs(plane_model[2] -
self.saved_plane_model[2]) <
self.abc_match_threshold
and abs(plane_model[3] -
self.saved_plane_model[3]) <
self.d_match_threshold
): #See if current plane is relatively close to previous plane
match = True #The walls match
else:
check = True #The walls don't match and we should find which is closer.
if len(
indices
) < self.robustThreshold: #If the wall isn't robust enough
print("No Robust Planes"
) #Print that the wall is bad
if self.saved_plane_model: #if there is a saved_plane_model
newdist = self.plane_distance(
self.saved_plane_model,
self.position) #Use saved_plane_model
self.linepoints = self.gettangentpoints(
self.saved_plane_model
) #Find points for line to draw in rviz
self.walldist = newdist #Set walldistance
break #skip to next iteration of while loop
if abs(
plane_model[2]
) < self.verticalityThreshold or match: #If the plane is vertical enough or the plane matches !!!(the "or match" may allow for finding a vertical wall that slopes into something horizontal.)
new_plane_dist = self.plane_distance(
plane_model, self.position
) #Find distance to newly found plane
if check: #if the current plane and previous plane were different (i.e. if check)
if abs(
new_plane_dist
) > self.walldist: #If the old plane was closer
plane_points = self.CurrP[
indices] #np.array(plist, dtype = np.float32)
if self.PassedPlane(
self.saved_plane_model,
plane_points, self.position
): #if newplane passes through where old plane was, then oldplane is gone.
usesave = False #Use the current plane
else:
usesave = True #Use the save
else: #otherwise
usesave = False #Use the current plane
if usesave: #If save is used
print("SAVE USED"
) #Print that save is used
newdist = self.plane_distance(
self.saved_plane_model,
self.position) #set distance
self.linepoints = self.gettangentpoints(
self.saved_plane_model
) #make line for rviz
self.walldist = newdist # set walldist
break #skip to next iteration of while loop
self.walldist = new_plane_dist #Set walldist to new plane distance
self.linepoints = self.gettangentpoints(
plane_model) #Make line for RVIZ
self.saved_plane_model = plane_model #set saved plane to the current plane
break #start next wall find
else: #if the plane didn't make the cut, and wasn't a wall
self.CurrP = np.delete(
self.CurrP, indices,
0) #delete all indices in the last wall.
if self.CurrP.shape[
0] < 5: #if there aren't enough points to continue
print('pointcloud too empty')
break
cloud_filtered = pcl.PointCloud(
self.CurrP
) # set new pcl pointcloud that uses the updated ROS pointcloud (which doesn't have the indices in the nonwall plane that was found)
#MAKE NEW SEGMENTER TO BE READY TO FIND THE PLANE
seg = cloud_filtered.make_segmenter()
seg.set_optimize_coefficients(True)
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
seg.set_distance_threshold(
self.planeDistThreshold)
continue #try to find another plane, ignore the one previously found
except Exception as inst: #If something goes wrong
#pass #ignore it
print inst #print it
print "wall_distance: " + str(abs(
self.walldist)) #print the current wall distance
self.dist_pub.publish(self.walldist)
if (self.visualized and self.linepoints):
self.vis_pub.publish(
Marker(header=Header(
frame_id="odom",
stamp=self.actualP.header.stamp),
type=Marker.LINE_LIST,
ns="current_plane",
scale=Vector3(x=.1, y=.1, z=.1),
points=self.linepoints,
colors=self.linecolors))
self.actualP = None #reset pointcloud, so as to not accidentally do the same pointcloud twice and wait for the next one from the phone
self.m.release()
r.sleep() #wait until next iteration
def setUp(self):
self.p = pcl.PointCloud(_data)
self.segment = pcl.MinCutSegmentation()
ros_pointnet2.stop_call()
exit(1)
signal.signal(signal.SIGINT, keyboardInterruptHandler)
while True:
# Create a pipeline object. This object configures the streaming camera and owns it's handle
frames = pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
rs_pc = rs.pointcloud()
points = rs_pc.calculate(depth_frame)
v = points.get_vertices()
verts = np.asanyarray(v).view(np.float32).reshape(-1, 3) # xyz
p = pcl.PointCloud()
p.from_array(np.array(verts, dtype=np.float32))
#print "original", p.size
#Downsampling
sor = p.make_voxel_grid_filter()
sor.set_leaf_size(0.05, 0.05, 0.05)
cloud_filtered = sor.filter()
#print "after Downsampling", cloud_filtered.size
#Passtrough filter
passthrough = cloud_filtered.make_passthrough_filter()
#passthrough = p.make_passthrough_filter()
passthrough.set_filter_field_name("z")
passthrough.set_filter_limits(0.0, 5.0)
cloud_filtered = passthrough.filter()
def preprocess_images(raw_color_im, raw_depth_im, camera_intr, T_camera_world,
workspace_box, workspace_im, image_proc_config):
""" Preprocess a set of color and depth images. """
# read params
inpaint_rescale_factor = image_proc_config['inpaint_rescale_factor']
cluster = image_proc_config['cluster']
cluster_tolerance = image_proc_config['cluster_tolerance']
min_cluster_size = image_proc_config['min_cluster_size']
max_cluster_size = image_proc_config['max_cluster_size']
# deproject into 3D world coordinates
point_cloud_cam = camera_intr.deproject(raw_depth_im)
point_cloud_cam.remove_zero_points()
point_cloud_world = T_camera_world * point_cloud_cam
# compute the segmask for points above the box
seg_point_cloud_world, _ = point_cloud_world.box_mask(workspace_box)
seg_point_cloud_cam = T_camera_world.inverse() * seg_point_cloud_world
depth_im_seg = camera_intr.project_to_image(seg_point_cloud_cam)
# mask out objects in the known workspace
env_pixels = depth_im_seg.pixels_farther_than(workspace_im)
depth_im_seg._data[env_pixels[:, 0], env_pixels[:, 1]] = 0
# REMOVE NOISE
# clip low points
low_indices = np.where(
point_cloud_world.data[2, :] < workspace_box.min_pt[2])[0]
point_cloud_world.data[2, low_indices] = workspace_box.min_pt[2]
# clip high points
high_indices = np.where(
point_cloud_world.data[2, :] > workspace_box.max_pt[2])[0]
point_cloud_world.data[2, high_indices] = workspace_box.max_pt[2]
# segment out the region in the workspace (including the table)
workspace_point_cloud_world, valid_indices = point_cloud_world.box_mask(
workspace_box)
invalid_indices = np.setdiff1d(np.arange(point_cloud_world.num_points),
valid_indices)
if cluster:
# create new cloud
pcl_cloud = pcl.PointCloud(
workspace_point_cloud_world.data.T.astype(np.float32))
tree = pcl_cloud.make_kdtree()
# find large clusters (likely to be real objects instead of noise)
ec = pcl_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(cluster_tolerance)
ec.set_MinClusterSize(min_cluster_size)
ec.set_MaxClusterSize(max_cluster_size)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract()
num_clusters = len(cluster_indices)
# read out all points in large clusters
filtered_points = np.zeros([3, workspace_point_cloud_world.num_points])
cur_i = 0
for j, indices in enumerate(cluster_indices):
num_points = len(indices)
points = np.zeros([3, num_points])
for i, index in enumerate(indices):
points[0, i] = pcl_cloud[index][0]
points[1, i] = pcl_cloud[index][1]
points[2, i] = pcl_cloud[index][2]
filtered_points[:, cur_i:cur_i + num_points] = points.copy()
cur_i = cur_i + num_points
# reconstruct the point cloud
all_points = np.c_[filtered_points[:, :cur_i],
point_cloud_world.data[:, invalid_indices]]
else:
all_points = point_cloud_world.data
filtered_point_cloud_world = PointCloud(all_points, frame='world')
# compute the filtered depth image
filtered_point_cloud_cam = T_camera_world.inverse(
) * filtered_point_cloud_world
depth_im = camera_intr.project_to_image(filtered_point_cloud_cam)
# form segmask
segmask = depth_im_seg.to_binary()
valid_px_segmask = depth_im.invalid_pixel_mask().inverse()
segmask = segmask.mask_binary(valid_px_segmask)
# inpaint
color_im = raw_color_im.inpaint(rescale_factor=inpaint_rescale_factor)
depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor)
return color_im, depth_im, segmask
def process_cloud(np_points,
camera_matrix,
pub_filtered_cloud,
initial_rotation,
filter_params,
publish_filtered=True):
'''
1. Transform to robot frame
2. Filter in z and x
3. Downsample
4. Publish and return filtered cloud
'''
# rospy.logdebug("Cloud received, frame : {}".format(cloud_msg.header.frame_id))
# pc = ros_numpy.numpify(cloud_msg)
# # pc_l = [np.asarray(x) for x in pc]
# # print(pc_l)
# # pc = np.asarray(pc_l)
# # height = pc.shape[0]
# # width = pc.shape[1]
# # np_points = np.zeros((height * width, 3), dtype=np.float32)
# np_points = np.zeros((pc.shape[0], 3), dtype=np.float32)
# np_points[:, 0] = np.resize(pc['x'], pc.shape[0])
# np_points[:, 1] = np.resize(pc['y'], pc.shape[0])
# np_points[:, 2] = np.resize(pc['z'], pc.shape[0])
# print(np_points.shape)
# import pdb
# pdb.set_trace()
# np_points_appened = np.hstack((np_points, np.ones((np_points.shape[0], 1))))
# np_points_transformed = np.matmul(total_transform, np.transpose(np_points_appened))
# np_points_transformed = np.transpose(np_points_transformed)[:,:3]
np_points_transformed = transform_cloud(np_points, mat=camera_matrix)
pcl_cloud = pcl.PointCloud(np_points_transformed)
passthrough = pcl_cloud.make_passthrough_filter()
passthrough.set_filter_field_name("z")
zmin = filter_params['zmin']
passthrough.set_filter_limits(zmin, 1.0)
cloud_filtered = passthrough.filter()
passthrough = cloud_filtered.make_passthrough_filter()
passthrough.set_filter_field_name("x")
passthrough.set_filter_limits(filter_params['xmin'], filter_params['xmax'])
cloud_filtered = passthrough.filter()
passthrough = cloud_filtered.make_passthrough_filter()
passthrough.set_filter_field_name("y")
passthrough.set_filter_limits(filter_params['ymin'], filter_params['ymax'])
cloud_filtered = passthrough.filter()
# fil = cloud_filtered.make_statistical_outlier_filter()
# fil.set_mean_k(100)
# fil.set_std_dev_mul_thresh(0.1)
# cloud_filtered = fil.filter()
sor = cloud_filtered.make_voxel_grid_filter()
sor.set_leaf_size(filter_params['downsampling_leaf_size'],
filter_params['downsampling_leaf_size'],
filter_params['downsampling_leaf_size'])
cloud_filtered = sor.filter()
fil = cloud_filtered.make_statistical_outlier_filter()
fil.set_mean_k(100)
fil.set_std_dev_mul_thresh(0.08)
cloud_filtered = fil.filter()
cloud_filtered_array = np.asarray(cloud_filtered)
# cloud_filtered_array = np_points
# cloud_filtered = pcl.PointCloud(np_points)
mean = np.mean(cloud_filtered_array, axis=0)
mean[2] = filter_params['zmin'] + filter_params['object_height'] / 2.0
pose_estimate = {}
# mean = mean
pose_estimate['location'] = mean.tolist()
# pose_estimate['quaternion'] = [0.02218474, -0.81609224, -0.57647473, 0.03432471]
# pose_estimate['quaternion'] = [0,0,0,1]
pose_estimate['quaternion'] = initial_rotation
print("Num points after downsample and filter in input cloud : {}".format(
cloud_filtered_array.shape[0]))
if publish_filtered:
cloud_color = np.zeros(cloud_filtered_array.shape[0])
ros_msg = xyzrgb_array_to_pointcloud2(cloud_filtered_array,
cloud_color, rospy.Time.now(),
"/base_footprint")
pub_filtered_cloud.publish(ros_msg)
rospy.logdebug("Done cloud processing")
return cloud_filtered, pose_estimate
def do_icp(cloud_in, camera_pose_matrix, pub_filtered_cloud, mesh_cloud,
pub_pose_cloud, pose_estimate):
# loc_scale = np.array([0.45, 1.5, 0.73])
# ori = [-1, 0, 0, 1]
loc_scale = pose_estimate['location']
ori = pose_estimate['quaternion']
ori_euler = tf.transformations.euler_from_quaternion(
pose_estimate['quaternion'])
R = tf.transformations.quaternion_matrix(ori)
T = tf.transformations.translation_matrix(loc_scale)
total_transform = tf.transformations.concatenate_matrices(T, R)
cloud_filtered_array = transform_cloud(mesh_cloud,
mat=np.copy(total_transform))
cloud_color = np.zeros(cloud_filtered_array.shape[0])
# mesh_cloud_cp = np.copy(mesh_cloud)
# ros_msg = xyzrgb_array_to_pointcloud2(
# cloud_filtered_array, cloud_color, rospy.Time.now(), "/base_footprint"
# )
# pub_pose_cloud.publish(ros_msg)
# print(total_transform)
# rospy.logwarn("Num points after downsample and filter : {}".format(cloud_filtered_array.shape[0]))
cloud_pose = pcl.PointCloud()
cloud_pose.from_array(cloud_filtered_array)
# print(cloud_filtered_array)
print("Doing ICP")
icp = cloud_pose.make_GeneralizedIterativeClosestPoint()
converged, transf, estimate, fitness = icp.gicp(cloud_pose,
cloud_in,
max_iter=10)
# icp = cloud_pose.make_IterativeClosestPoint()
# converged, transf, estimate, fitness = icp.icp(cloud_pose, cloud_in)
print('has converged:' + str(converged) + ' score: ' + str(fitness))
print(str(transf))
total_transform_icp = tf.transformations.concatenate_matrices(
transf, total_transform)
print(str(total_transform_icp))
pose_output = {}
pose_output['quaternion'] = tf.transformations.quaternion_from_matrix(
total_transform_icp)
pose_output['location'] = tf.transformations.translation_from_matrix(
total_transform_icp)
pose_output['matrix'] = total_transform_icp
# Keep only x,y, and yaw from ICP result
yaw_icp = math.atan2(transf[1, 0], transf[0, 0])
yaw_in = ori_euler[2]
cos_term = math.cos(yaw_in + yaw_icp)
sin_term = math.sin(yaw_in + yaw_icp)
total_yaw = math.atan2(sin_term, cos_term)
pose_output['quaternion'] = tf.transformations.quaternion_from_euler(
ori_euler[0], ori_euler[1], total_yaw)
pose_output['location'][2] = loc_scale[2]
R = tf.transformations.quaternion_matrix(pose_output['quaternion'])
T = tf.transformations.translation_matrix(pose_output['location'])
total_transform_icp = tf.transformations.concatenate_matrices(T, R)
total_transform_icp_cam = \
tf.transformations.concatenate_matrices(camera_pose_matrix, total_transform_icp)
print(total_transform_icp_cam)
pose_output_cam = {}
pose_output_cam['quaternion'] = tf.transformations.quaternion_from_matrix(
total_transform_icp_cam)
pose_output_cam['location'] = tf.transformations.translation_from_matrix(
total_transform_icp_cam)
pose_output_cam['matrix'] = total_transform_icp_cam
cloud_filtered_array = transform_cloud(mesh_cloud, mat=total_transform_icp)
cloud_color = np.zeros(cloud_filtered_array.shape[0])
ros_msg = xyzrgb_array_to_pointcloud2(cloud_filtered_array, cloud_color,
rospy.Time.now(), "/base_footprint")
pub_pose_cloud.publish(ros_msg)
return pose_output, pose_output_cam
"""
ax3.scatter(traj[0,2],
traj[1,2],
traj[2,2],
color='r')
"""
ax3.scatter(-dataset_raw.calib.T_cam0unrect_velo[0, 3],
-dataset_raw.calib.T_cam0unrect_velo[1, 3],
-dataset_raw.calib.T_cam0unrect_velo[2, 3],
color='r')
plt.show()
source_points = velo_points_cframe[0:3, :].T
source_points = source_points.astype(np.float32)
pc_source = pcl.PointCloud()
pc_source.from_array(source_points)
target_points = velo_points_wframe[0:3, :].T
target_points = target_points.astype(np.float32)
pc_target = pcl.PointCloud()
pc_target.from_array(target_points)
_, alignment, _, _ = icp(pc_source, pc_target)
f5 = plt.figure()
ax5 = f5.add_subplot(111, projection='3d')
aligned_source = np.dot(alignment, velo_points_cframe)
ax5.scatter(aligned_source[0, :],
def filtering_raw_data(self, data):
# plot original data
if self.debug:
self.plot_point_cloud('point_clouds', data) # <-- plot
####
# Removing Table <-- this code will be removed on real practice
if not self.flip:
data_y = data[:, 1] # normal
else:
data_y = data[:, 0] # flip
data = data[np.where((data_y >= 0.4))]
# self.plot_point_cloud('point_clouds', data) # <-- plot
####
# flipping data between X Axes & Y Axes
if not self.flip:
data_y = data[:, 1] # normal
else:
data_y = data[:, 0] # flip
# Sorting point cloud
data = data[np.argsort(data_y)] # Y axis
if not self.flip:
data_y = data[:, 1] # normal
else:
data_y = data[:, 0] # flip
# square scaning
ymin = np.round(np.min(data_y), 1) - 0.1
ymax = np.round(np.max(data_y), 1) + 0.1
y_step_size = 0.025 #0.01
y_step = np.arange(ymin, ymax, y_step_size)
# rospy.loginfo('[CAL] Y Point: {}'.format( (ymin, ymax) ))
# print(y_step)
# print()
z_list = []
z_prev = None
for y in range(len(y_step)):
y_ll = y_step[y]
y_ul = y_step[y] + y_step_size
# print('Y : {:.3f}, {:.3f}'.format(y_ll, y_ul) )
layer = data[np.where((data_y >= y_ll) & (data_y < y_ul))]
if layer.size != 0:
zmax = np.max(layer[:, 2])
# print('\t\ttotal data: {}, max z : {:.4f}'.format(len(layer), zmax ) )
z_list.append(zmax)
if z_prev != None:
diff = zmax - z_prev
if diff <= -0.5:
y_lim = y_ll
break
z_prev = zmax
# print()
if self.debug:
_, axes2D = plt.subplots(nrows=1, ncols=1)
self.plots_(axes2D, y_step[:len(z_list)], z_list, legend_=None, scatter=False, \
xlabel_="y-layer", ylabel_="z-height", title_="Filtering Human Body on Point Cloud")
human_body = data[np.where((data_y >= ymin) & (data_y < y_lim))]
flag_human_body = human_body
# self.plot_point_cloud('human_body', human_body) # <-- plot
# Reference point
if not self.flip:
x_ref = (np.min(human_body[:, 0]) + np.max(human_body[:, 0])) / 2
y_ref = np.min(human_body[:, 1])
else:
x_ref = np.min(human_body[:, 0])
y_ref = (np.min(human_body[:, 1]) + np.max(human_body[:, 1])) / 2
z_ref = np.max(human_body[:, 2])
ref_point = np.array([[x_ref, y_ref, z_ref]])
# rospy.loginfo('[CAL] Ref. Point: {}'.format(ref_point))
# Euclidean Distance of 3D Point
eucl_dist = np.linalg.norm(ref_point - human_body[:, :3], axis=1)
try:
# Filter euclidean distance
human_body = human_body[np.where((eucl_dist <= 0.8))] #0.8
# self.plot_point_cloud('human_body', human_body, big_point=True, color=True )
p = pcl.PointCloud(np.array(human_body[:, :3], dtype=np.float32))
sor = p.make_voxel_grid_filter()
sor.set_leaf_size(0.01, 0.01, 0.01)
filtered = sor.filter()
filtered_human_body = np.asarray(filtered)
if self.debug:
self.plot_point_cloud(
'filtered_human_body',
filtered_human_body) #, big_point=True, color=True )
return filtered_human_body
except:
if self.debug:
self.plot_point_cloud(
'human_body',
flag_human_body) #, big_point=True, color=True )
return flag_human_body
def find_object(pc, obj_dims, ball_radius):
npc = pc.to_array()
# Crop to cylinder segment
print('Finding object (#PC {})'.format(pc.size))
npc = npc[(npc[:, 2] > 0.4) * (npc[:, 2] < 1.5)
# * (np.sum(npc[:, :2]**2, axis=1) < 0.5**2)
]
pc = pcl.PointCloud(npc)
pcl.save(pc, 'tmp/scene_cropped.pcd')
print('Cropped (#PC {})'.format(pc.size))
# Downsample
point_distance = 0.01
face_density = 1 / point_distance**2
vg = pc.make_voxel_grid_filter()
vg.set_leaf_size(*(3 * [point_distance]))
pc = vg.filter()
npc = pc.to_array()
pcl.save(pc, 'tmp/scene_downsampled.pcd')
print('Downsampled (#PC {})'.format(pc.size))
# Remove extension of planes of considerable goodness
psegm = PlaneSegmenter(distance_tolerance=0.001,
normal_distance_weight=0.1,
consume_distance=0.005,
maximum_iterations=1e4,
minimum_plane_points=500,
minimum_density=0.3 * face_density)
planes, pc = psegm(pc)
npc = pc.to_array()
pcl.save(pc, 'tmp/scene_curved.pcd')
print('Filtered planes (#PC {})'.format(pc.size))
# Remove isolated points
sor = pc.make_statistical_outlier_filter()
sor.set_mean_k(10)
sor.set_std_dev_mul_thresh(0.1)
pc = sor.filter()
print('#PC: {}'.format(pc.size))
npc = pc.to_array()
pcl.save(pc, 'tmp/scene_dense.pcd')
print('Filtered outliers (#PC {})'.format(pc.size))
# Finding three spheres directly in scene:
# spheres = find_spheres(pc, n_max=3)
# Finding spheres based on Euclidean Clustering
ece = EuclideanClusterExtractor(
nn_dist=2 * point_distance, # ball_radius*(1-np.cos(np.pi/6)),
min_pts=10,
min_max_length=0.3 * ball_radius,
max_max_length=2.3 * ball_radius)
ecs = ece.extract(pc)
print('Extracted {} clusters'.format(len(ecs)))
if len(ecs) == 0:
print('!!!!!!!\n!!!!!! No Clusters found !!!!!!\n!!!!!!!\n')
return None
spheres = []
for i, ec in enumerate(ecs):
pcl.save(ec, 'tmp/cluster_{:03d}.pcd'.format(i))
ec_spheres = find_spheres(ec, ball_radius, n_max=1)
spheres += ec_spheres
spheres = [sph for sph in spheres if sph[0].size > 10]
n_sph = len(spheres)
spheres.sort(key=lambda s: s[0].size, reverse=True)
for i in range(n_sph):
pcl.save(spheres[i][0], 'tmp/sphere_{}.pcd'.format(i))
centres = [m3d.Vector(sph[1][:3]) for sph in spheres]
n_centres = len(centres)
# Setup distance matrix
distm = spsp.distance.squareform(
spsp.distance.pdist([c.array for c in centres]))
# distm = np.zeros((n_centres,n_centres))
# for i in range(n_centres):
# for j in range(n_centres):
# distm[i,j] = (centres[i] - centres[j]).length
print(distm)
# Match matrix
matchm0 = np.abs(distm - obj_dims[0]) < 0.01
matchm1 = np.abs(distm - obj_dims[1]) < 0.01
match = np.logical_and(matchm0.any(axis=1), matchm1.any(axis=1))
# Test if there is unambiguous object match
matchidxs = np.where(match)[0]
if len(matchidxs) > 1:
print('!!!!!! Match was ambiguous (#matches={}) !!!!!!\n'.format(
len(matchidxs)))
return None
if len(matchidxs) == 0:
print('!!!!!!!\n!!!!!! No match found. !!!!!!\n!!!!!!!\n')
return None
# Identify indexes for sphere 0
sph_0_idx = matchidxs[0]
# Sphere x and y indexes where distances match
errs_x = np.abs(distm[sph_0_idx] - obj_dims[0])
errs_y = np.abs(distm[sph_0_idx] - obj_dims[1])
sph_x_idx = errs_x.argmin()
sph_y_idx = errs_y.argmin()
err_x = errs_x[sph_x_idx]
err_y = errs_y[sph_y_idx]
print('Matching indices: 0:{}, x:{} ({}), y:{} ({})'.format(
sph_0_idx, sph_x_idx, err_x, sph_y_idx, err_y))
# Single out the centres and form the x- and y-unit vectors.
sph_0 = centres[sph_0_idx]
sph_x = centres[sph_x_idx]
sph_y = centres[sph_y_idx]
d_x = (sph_x - sph_0).normalized
d_y = (sph_y - sph_0).normalized
# Form the object transform
t_obj = m3d.Transform.new_from_xyp(d_x, d_y, sph_0)
return t_obj
def setUp(self):
self.p = pcl.PointCloud(_data)
def AllPoses(
self,
save_poses=False,
dir_name=None,
align_pc=False): #read a message, display pose, and write to file
print('Showing All Poses from Robot in openRAVE')
save_time_interval = 0.1 # amount of time between saved stls
# some directory checking for file creation
if save_poses:
if dir_name is not None:
try:
shutil.rmtree(dir_name)
print("Folder Exists. Deleting Folder")
except:
print("Folder Did not Exist")
os.mkdir(dir_name, 0777)
else:
print("no directory name")
return
#read all messages
#if there is a timestamp with enough information, show the pose
# if there is a timestamp with enough information, save the pointcloud data from that timestep
bridge = CvBridge()
last_save_time = 0
for topic, msg, t in self.bag_gen:
self.parseData(topic, msg,
t) #adds message to all_data with topic as key
if '/wam/joint_states' in self.all_data[-1].keys(
) and '/bhand/joint_states' in self.all_data[-1].keys():
# pdb.set_trace()
# if the necessary data is in the last entry of list to show a pose
JA = self.robotStateToArray(
self.all_data[-1]['/wam/joint_states'],
self.all_data[-1]['/bhand/joint_states'])
# pdb.set_trace()
self.setJA(JA)
print('Pose at time: %s' % self.all_data[-1]['time'])
time.sleep(0.01)
if save_poses:
if '/camera1/depth/points' in self.all_data[-1].keys() \
and '/camera1/rgb/image_raw/compressed' in self.all_data[-1].keys() \
and '/camera2/depth/points' in self.all_data[-1].keys() \
and '/camera2/rgb/image_raw/compressed' in self.all_data[-1].keys() \
and abs(self.all_data[-1]['time'] - last_save_time) > save_time_interval:
# if '/kinect2/hd/points' in self.all_data[-1].keys() and '/camera/depth/points' in self.all_data[-1].keys():
camera1_pts = self.pc2ToXYZRGB(
self.all_data[-1]['/camera1/depth/points'])
camera2_pts = self.pc2ToXYZRGB(
self.all_data[-1]['/camera2/depth/points'])
# camera1_pts = self.pc2ToXYZ(self.all_data[-1]['/camera1/depth/points'])
# camera2_pts = self.pc2ToXYZ(self.all_data[-1]['/camera2/depth/points'])
self.showPointCloud(camera1_pts)
self.showPointCloud(camera2_pts)
# pdb.set_trace()
# saving image from camera -- should check this at the if statement...
try:
cv_image1 = bridge.compressed_imgmsg_to_cv2(
self.all_data[-1]
['/camera1/rgb/image_raw/compressed'], "bgr8")
cv_image2 = bridge.compressed_imgmsg_to_cv2(
self.all_data[-1]
['/camera2/rgb/image_raw/compressed'], "bgr8")
except CvBridgeError, e:
print e
# save point clouds
camera_pts = [camera1_pts, camera2_pts]
# camera_pts = [camera1_pts]
camera_filename = [
'%s/frame%s_pc%s.csv' %
(dir_name, self.frame_count, ci)
for ci in range(len(camera_pts))
]
for i in range(len(camera_pts)
): # won't work with only one camera
with open(camera_filename[i], 'wb') as f:
cam_writer = csv.writer(f, delimiter=',')
for pt in camera_pts[i]:
cam_writer.writerow(pt)
print("CSV File Created: %s" % camera_filename[i])
# save STL
self.arm.generateSTL('%s/frame%s.stl' %
(dir_name, self.frame_count))
# save image
cv2.imwrite(
'%s/frame%s_camera%s.png' %
(dir_name, self.frame_count, 1), cv_image1)
cv2.imwrite(
'%s/frame%s_camera%s.png' %
(dir_name, self.frame_count, 2), cv_image2)
self.frame_count += 1
last_save_time = self.all_data[-1]['time']
if align_pc:
# check if data is there
if '/camera1/depth/points' in self.all_data[-1].keys() \
and '/camera2/depth/points' in self.all_data[-1].keys() :
# get mesh from STL
self.arm.getSTLFeatures()
# convert to proper format
pcarm = pcl.PointCloud()
pcarm.from_list(self.arm.all_vertices)
# get data
camera1_pts = np.array(
self.pc2ToXYZRGB(
self.all_data[-1]['/camera1/depth/points']))
camera2_pts = np.array(
self.pc2ToXYZRGB(
self.all_data[-1]['/camera2/depth/points']))
# convert to proper format
pc1 = pcl.PointCloud()
pc2 = pcl.PointCloud()
# seperate from colors
pc1.from_list(camera1_pts[:, 0:3])
pc2.from_list(camera2_pts[:, 0:3])
pdb.set_trace()
if pc1.size == 0 and pc2.size == 0:
print('No Points to align')
elif pc1.size != 0 and pc2.size == 0:
print("Aligning PC1 with arm")
icp_success1, pc1_T, pc1_transfomed, fitness1 = pcl.registration.icp_nl(
source=pc1, target=pcarm, max_iter=None)
pc1_transformed_list = np.hstack(
(pc1_transformed.to_list(),
camera1_pts[:, 3:])) # reassemble with colors
self.showPointCloud(pc1_transformed_list)
elif pc1.size == 0 and pc2.size != 0:
print("Aligning PC2 with arm")
icp_success2, pc2_T, pc2_transformed, fitness2 = pcl.registration.icp_nl(
source=pc2, target=pcarm, max_iter=None)
pc2_transformed_list = np.hstack(
(pc2_transformed.to_list(),
camera2_pts[:, 3:])) # reassemble with colors
self.showPointCloud(pc2_transformed_list)
elif pc1.size != 0 and pc2.size != 0:
print("Aligning PC1 to PC2"
) # align point clouds to other point cloud
icp_success12, pc12_T, pc12_transfomed, fitness12 = pcl.registration.icp_nl(
source=pc1, target=pc2, max_iter=None)
pc12_transformed_list = np.hstack(
(pc12_transformed.to_list(),
camera1_pts[:, 3:])) # reassemble with colors
self.showPointCloud(pc12_transformed_list)
else:
print("Should not be here...")
for idx, frame in enumerate(self.all_data):
if frame['time'] < self.all_data[-1][
'time']: #if entry time is less than current time, pop
self.all_data.pop(idx)
print('List Entry Popped')
def cal_grasp(msg, cam_pos_):
"""根据在线采集的点云计算候选的抓取姿态
"""
#把pointcloud2类型的消息点云，转换为ndarray points_
points_ = pointclouds.pointcloud2_to_xyz_array(msg)
#复制一份points_ ndarray对象，并将所有的点坐标转换为float32类型
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_voxel_ = get_voxel_fun(points_, n)
#当点云点数小于2000时
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_)
print(len(points_))
#构造法向量估计对象
norm = point_cloud.make_NormalEstimation()
tree = point_cloud.make_kdtree()
norm.set_SearchMethod(tree)
#以周边30个点作为法向量计算点
norm.set_KSearch(10) # critical parameter when calculating the norms
normals = norm.compute()
#将点云法向量转换为ndarry类型
surface_normal = normals.to_array()
surface_normal = surface_normal[:, 0:3]
#每个点到 相机位置（无姿态）的向量 但是，感觉是相机到点的向量
vector_p2cam = cam_pos_ - points_
#print(vector_p2cam)
#print(cam_pos_)
"""
np.linalg.norm(vector_p2cam, axis=1) 默认求2范数，axis=1 代表按行向量处理，求多个行向量的2范数（求模）
np.linalg.norm(vector_p2cam, axis=1).reshape(-1, 1) 将其调整为m行 1列
整句话的含义是，将vector_p2cam归一化，单位化
"""
vector_p2cam = vector_p2cam / np.linalg.norm(vector_p2cam, axis=1).reshape(
-1, 1)
#将表面法相与表面法相（都是单位向量）点乘，以备后面计算向量夹角
tmp = np.dot(vector_p2cam, surface_normal.T).diagonal()
#print(vector_p2cam)
#print(surface_normal.T)
#print(tmp)
"""
np.clip(tmp, -1.0, 1.0) 截取函数，将tmp中的值，都限制在-1.0到1.0之间，大于1的变成1，小于-1的记为-1
np.arccos() 求解反余弦，求夹角
"""
angel = np.arccos(np.clip(tmp, -1.0, 1.0))
#print(angel)
#找到与视角向量夹角大于90度的角（认为法向量计算错误）
wrong_dir_norm = np.where(angel > np.pi * 0.5)[0]
#print(np.where(angel > np.pi * 0.5))
#print(wrong_dir_norm)
#print(len(wrong_dir_norm))
#创建一个len(angel)行，3列的ndarry对象
tmp = np.ones([len(angel), 3])
#将法向量错误的行的元素都改写为-1
tmp[wrong_dir_norm, :] = -1
#与表面法相元素对元素相乘，作用是将"错误的"法向量的方向 扭转过来
surface_normal = surface_normal * tmp
#选取桌子以上2cm处的点作为检测点
select_point_above_table = 0.020
#modify of gpg: make it as a parameter. avoid select points near the table.
#查看每个点的z方向，如果它们的点z轴方向的值大于select_point_above_table，就把他们抽出来
points_for_sample = points_[np.where(
points_[:, 2] > select_point_above_table)[0]]
print(len(points_for_sample))
if len(points_for_sample) == 0:
rospy.loginfo(
"Can not seltect point, maybe the point cloud is too low?")
return [], points_, surface_normal
yaml_config['metrics']['robust_ferrari_canny']['friction_coef'] = value_fc
grasps_together_ = []
if rospy.get_param("/robot_at_home") == "false":
robot_at_home = False
else:
robot_at_home = True
if not robot_at_home:
rospy.loginfo("Robot is moving, waiting the robot go home.")
elif 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_single_worker,
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=False)
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!")
if show_final_grasp and using_mp:
ags.show_all_grasps(points_, grasps_together_)
ags.show_points(points_, scale_factor=0.002)
mlab.show()
rospy.loginfo("Grasp sampler finish, generated {} grasps.".format(
len(grasps_together_)))
#返回抓取 场景的点 以及点云的表面法向量
return grasps_together_, points_, surface_normal
manager = cluster_manager(debug=args.debug)
for i, velo in enumerate(dataset.velo):
if len(tracklet_rects[i]) == 0: continue
#if i%30!=0:
# continue
oxts_pose = next(iter(itertools.islice(dataset.oxts, i, None))).T_w_imu
oxts_pose = oxts_pose.dot(np.linalg.inv(
dataset.calib.T_velo_imu)) # world-velo
# register points to world coord
velo = np.hstack((velo[:, :3], np.ones(
(velo.shape[0], 1)))).dot(oxts_pose.T)
# vox
velo = pcl.PointCloud(velo[:, :3].astype(np.float32))
sor = velo.make_voxel_grid_filter()
sor.set_leaf_size(0.1, 0.1, 0.1)
velo = sor.filter().to_array()
# register robot center
cen = np.vstack((cen, oxts_pose.dot([0, 0, 0, 1])))
velo = filter_ground(velo, cen, th=15)
labels = cluster_points(velo, n_clusters=n_clusters)
if args.debug:
manager.update(velo, labels)
# evaluation
for j, box in enumerate(tracklet_rects[i]):
def testCropHull(self):
filterCloud = pcl.PointCloud()
vt = pcl.Vertices()
def main():
try:
from pylibfreenect2 import OpenCLPacketPipeline
pipeline = OpenCLPacketPipeline()
except:
from pylibfreenect2 import CpuPacketPipeline
pipeline = CpuPacketPipeline()
# Create and set logger
logger = createConsoleLogger(LoggerLevel.Debug)
setGlobalLogger(logger)
fn = Freenect2()
num_devices = fn.enumerateDevices()
if num_devices == 0:
print("No device connected!")
sys.exit(1)
serial = fn.getDeviceSerialNumber(0)
device = fn.openDevice(serial, pipeline=pipeline)
listener = SyncMultiFrameListener(FrameType.Color | FrameType.Ir
| FrameType.Depth)
# Register listeners
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.start()
# NOTE: must be called after device.start()
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
# Optinal parameters for registration
# set True if you need
need_bigdepth = False
need_color_depth_map = False
bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None
color_depth_map = np.zeros((424, 512), np.int32).ravel() \
if need_color_depth_map else None
point = pcl.PointCloud()
visual = pcl.pcl_visualization.CloudViewing()
visual.ShowColorCloud(cloud)
while True:
frames = listener.waitForNewFrame()
color = frames["color"]
ir = frames["ir"]
depth = frames["depth"]
registration.apply(color,
depth,
undistorted,
registered,
bigdepth=bigdepth,
color_depth_map=color_depth_map)
# NOTE for visualization:
# cv2.imshow without OpenGL backend seems to be quite slow to draw all
# things below. Try commenting out some imshow if you don't have a fast
# visualization backend.
# cv2.imshow("ir", ir.asarray() / 65535.)
# cv2.imshow("depth", depth.asarray() / 4500.)
# cv2.imshow("color", cv2.resize(color.asarray(), (int(1920 / 3), int(1080 / 3))))
# cv2.imshow("registered", registered.asarray(np.uint8))
# if need_bigdepth:
# cv2.imshow("bigdepth", cv2.resize(bigdepth.asarray(np.float32),
# (int(1920 / 3), int(1082 / 3))))
# if need_color_depth_map:
# cv2.imshow("color_depth_map", color_depth_map.reshape(424, 512))
undistorted_arrray = undistorted.asarray(dtype=np.float32, ndim=2)
# registered_array = registered.asarray(dtype=np.uint8)
point = pcl.PointCloud(undistorted_arrray)
# visual.ShowColorCloud(cloud)
listener.release(frames)
# key = cv2.waitKey(delay=1)
# if key == ord('q'):
# break
v = True
while v:
v = not (visual.WasStopped())
device.stop()
device.close()
sys.exit(0)
def setUp(self):
self.p = pcl.PointCloud(_data)
self.segment = pcl.ProgressiveMorphologicalFilter()
file_count += 1
for model_index in range(len(data_test)):
test_file = open(
OUT_DIR + "/test/" + file_name + str(file_count) + ".txt", 'w')
test_data_file = open(
OUT_DIR + "/test_data/" + file_name + str(file_count) + ".pts",
'w')
test_label_file = open(
OUT_DIR + "/test_label/" + file_name + str(file_count) + ".seg",
'w')
try:
test_cloud = pcl.PointCloud(data_train[model_index])
print(test_cloud)
pcl.save(test_cloud, (OUT_DIR + "/test_ply/" + file_name +
str(file_count) + ".ply"),
format="ply")
except:
print("No Data, skipping.")
continue
for data in range(len(data_test[model_index])):
data_str = str(data_test[model_index][data][0]) + " " + str(
data_test[model_index][data][1]) + " " + str(
data_test[model_index][data][2])
normal_str = str(normal_test[model_index][data][0]) + " " + str(
normal_test[model_index][data][1]) + " " + str(
normal_test[model_index][data][2])
def setUp(self):
self.p = pcl.PointCloud(_data)
self.segment = pcl.ConditionalEuclideanClustering()
def cal_grasp(msg, cam_pos_):
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 colibrated, here is a work around
points_[:,
2] = points_[:,
2] #+ 0.018 # liang: as the kinect2 is not well colibrated, here is a work around
points_voxel_ = get_voxel_fun(points_, n)
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_)
norm = point_cloud.make_NormalEstimation()
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 seltect 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
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np
import pcl
cloud = pcl.PointCloud()
cloud._from_ply_file("E:/CloudMethod/Changed/000.ply")
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
clouds = np.array(cloud.to_array(),dtype=np.float32)
# For each set of style and range settings, plot n random points in the box
# defined by x in [23, 32], y in [0, 100], z in [zlow, zhigh].
XT = clouds[:,0]
YT = clouds[:,1]
ZT = clouds[:,2]
X = XT[1::20]
Y = YT[1::20]
Z = ZT[1::20]
print X,Y,Z
ax.scatter(X, Y, Z , s=0.5 ,c='r')
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
plt.show()
def gen_labels(self, FLAGS, scan_name, label_name, label_output_dir):
# start = time.time()
# open scan
# TODO(yxm): downsampling
scan = np.fromfile(scan_name, dtype=np.float32)
scan = scan.reshape((-1, 4))
# put in attribute
points = scan[:, 0:4] # get xyzr
remissions = scan[:, 3] # get remission
label = np.fromfile(label_name, dtype=np.uint32)
label = label.reshape((-1))
# demolition or not
if FLAGS.demolition == True:
start_angle = np.random.random()
start_angle *= 360
end_angle = (start_angle + drop_angle) % 360
angle = np.arctan2(points[:, 1], points[:, 0])
angle = angle * 180 / np.pi
angle += 180
# print("angle:", angle)
if end_angle > start_angle:
remain_id = np.argwhere(angle < start_angle).reshape(-1)
remain_id = np.append(
remain_id,
np.argwhere(angle > end_angle).reshape(-1))
else:
remain_id = np.argwhere((angle > end_angle)
& (angle < start_angle)).reshape(-1)
points = points[remain_id, :]
label = label[remain_id]
if label.shape[0] == points.shape[0]:
sem_label = label & 0xFFFF # semantic label in lower half
inst_label = label >> 16 # instance id in upper half
assert ((sem_label + (inst_label << 16) == label).all())
else:
print("Points shape: ", points.shape)
print("Label shape: ", label.shape)
raise ValueError(
"Scan and Label don't contain same number of points")
sem_label = remap_lut[sem_label]
sem_label_set = list(set(sem_label))
sem_label_set.sort()
# Start clustering
cluster = []
inst_id = 0
for id_i, label_i in enumerate(sem_label_set):
# print('sem_label:', label_i)
index = np.argwhere(sem_label == label_i)
index = index.reshape(index.shape[0])
sem_cluster = points[index, :]
# print("sem_cluster_shape:",sem_cluster.shape[0])
tmp_inst_label = inst_label[index]
tmp_inst_set = list(set(tmp_inst_label))
tmp_inst_set.sort()
# print(tmp_inst_set)
if label_i in [9, 10]: # road/parking, dont need to cluster
inst_cluster = sem_cluster
inst_cluster = np.concatenate(
(inst_cluster,
np.full(
(inst_cluster.shape[0], 1), label_i,
dtype=np.uint32)),
axis=1)
# inst_cluster = np.insert(inst_cluster, 4, label_i, axis=1)
inst_cluster = np.concatenate(
(inst_cluster,
np.full(
(inst_cluster.shape[0], 1), inst_id,
dtype=np.uint32)),
axis=1)
inst_id = inst_id + 1
cluster.extend(inst_cluster) # Nx6
continue
elif label_i in [0, 2, 3, 6, 7, 8]: # discard
continue
elif len(tmp_inst_set) > 1 or (
len(tmp_inst_set) == 1
and tmp_inst_set[0] != 0): # have instance labels
for id_j, label_j in enumerate(tmp_inst_set):
points_index = np.argwhere(tmp_inst_label == label_j)
points_index = points_index.reshape(points_index.shape[0])
# print(id_j, 'inst_size:', len(points_index))
if len(points_index) <= 20:
continue
inst_cluster = sem_cluster[points_index, :]
inst_cluster = np.concatenate(
(inst_cluster,
np.full((inst_cluster.shape[0], 1),
label_i,
dtype=np.uint32)),
axis=1)
# inst_cluster = np.insert(inst_cluster, 4, label_i, axis=1)
inst_cluster = np.concatenate(
(inst_cluster,
np.full((inst_cluster.shape[0], 1),
inst_id,
dtype=np.uint32)),
axis=1)
inst_id = inst_id + 1
cluster.extend(inst_cluster)
else: # Euclidean cluster
# time_start = time.time()
if label_i in [1, 4, 5, 14]: # car truck other-vehicle fence
cluster_tolerance = 0.5
elif label_i in [
11, 12, 13, 15, 17
]: # sidewalk other-ground building vegetation terrain
cluster_tolerance = 2
else:
cluster_tolerance = 0.2
if label_i in [16, 19]: # trunk traffic-sign
min_size = 50
elif label_i == 15: # vegetation
min_size = 200
elif label_i in [11, 12, 13,
17]: # sidewalk other-ground building terrain
min_size = 300
else:
min_size = 100
# print(cluster_tolerance, min_size)
cloud = pcl.PointCloud(sem_cluster[:, 0:3])
tree = cloud.make_kdtree()
ec = cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(cluster_tolerance)
ec.set_MinClusterSize(min_size)
ec.set_MaxClusterSize(50000)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract()
# time_end = time.time()
# print(time_end - time_start)
for j, indices in enumerate(cluster_indices):
# print('j = ', j, ', indices = ' + str(len(indices)))
inst_cluster = np.zeros((len(indices), 4),
dtype=np.float32)
inst_cluster = sem_cluster[np.array(indices), 0:4]
inst_cluster = np.concatenate(
(inst_cluster,
np.full((inst_cluster.shape[0], 1),
label_i,
dtype=np.uint32)),
axis=1)
# inst_cluster = np.insert(inst_cluster, 4, label_i, axis=1)
inst_cluster = np.concatenate(
(inst_cluster,
np.full((inst_cluster.shape[0], 1),
inst_id,
dtype=np.uint32)),
axis=1)
inst_id = inst_id + 1
cluster.extend(inst_cluster) # Nx6
# print(time.time()-start)
# print('*'*80)
cluster = np.array(cluster)
if 'path' in FLAGS.pub_or_path:
np.save(
label_output_dir + '/' +
label_name.split('/')[-1].split('.')[0] + ".npy", cluster)
if 'pub' in FLAGS.pub_or_path:
# print(cluster[11100:11110])
msg_points = pc2.create_cloud(header=self.header1,
fields=_make_point_field(
cluster.shape[1]),
points=cluster)
self._labels_pub.publish(msg_points)
return cluster
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import pcl
from pcl.registration import icp, gicp, icp_nl
theta = [0., 0., np.pi]
rot_x = [[1, 0, 0], [0, cos(theta[0]), -sin(theta[0])],
[0, sin(theta[0]), cos(theta[0])]]
rot_y = [[cos(theta[1]), 0, sin(theta[1])], [0, 1, 0],
[-sin(theta[1]), 0, cos(theta[1])]]
rot_z = [[cos(theta[2]), -sin(theta[1]), 0], [sin(theta[2]),
cos(theta[1]), 0], [0, 0, 1]]
transform = np.dot(rot_x, np.dot(rot_y, rot_z))
source = np.random.randn(100, 3)
source_pc = pcl.PointCloud(source.astype(np.float32))
target = np.dot(transform, source.T).T
target_pc = pcl.PointCloud(target.astype(np.float32))
converged, transf, estimate, fitness = icp(source_pc,
target_pc,
max_iter=10000)
print(converged, transf, estimate, fitness)
result = estimate.to_array()
fig = plt.figure()
ax = fig.add_subplot(221, projection='3d')
ax.scatter(source[:, 0], source[:, 1], source[:, 2])
ax = fig.add_subplot(222, projection='3d')
ax.scatter(target[:, 0], target[:, 1], target[:, 2])
def write_pcd_file(pointcloud, output_path):
output_pointcloud = pcl.PointCloud()
output_pointcloud.from_array(np.float32(pointcloud))
output_pointcloud.to_file(output_path)
return
def add_pose_estimation(bag):
v_z = Quaternion(0, 0, 0, 1)
# orientation of the imu frame
imu_frame = Quaternion(-0.0198769629216, 0.974536168168, -0.218396560265, -0.0467665023439)
x_vec = np.array([1, 0, 0])
time = []
previous_pcl = pcl.PointCloud()
current_pcl = pcl.PointCloud()
est_pose_bool = False
pose_msg = PoseWithCovarianceStamped()
pose_msg.header.frame_id = 'base_footprint'
previous_pose = np.array([])
previous_ori = []
x = []
y = []
prev_tr = np.array([])
cov = [0] * 36
scan_time = []
ros_time = []
f_pose = 20 # Hz
queued_pcl_list = []
dt = 0
print 'Extracting poses using ICP'
for topic, msg, t in bag.read_messages(topics=['/imu/data', '/scan'],
start_time=rospy.Time(bag.get_start_time())+rospy.Duration(0)):
#
if topic == '/imu/data':
q = Quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z)
v_z_ = q * imu_frame * v_z * imu_frame.conjugate * q.conjugate
imu_rot = q
if np.arccos(v_z_[3])-math.pi > -.05:
est_pose_bool = True
else:
est_pose_bool = False
# EDIT
est_pose_bool = True
elif topic == "/scan" and est_pose_bool:
previous_pcl.from_array(current_pcl.to_array())
current_pcl.from_array(to_pcl_array(msg))
if dt != 0:
if dt > 1:
queued_pcl_list = []
else:
queued_pcl_list.append(previous_pcl)
if len(queued_pcl_list) > 1: #queue size = 2 less than 1521*2*3 points
queued_pcl_list.pop(0)
previous_pcl_array = np.concatenate(([queue_.to_array() for queue_ in queued_pcl_list]), axis=0)
previous_pcl.from_array(previous_pcl_array)
if not time:
time.append(msg.header.stamp.to_sec())
prev_tr = np.zeros((1, 3))
scan_time.append(msg.header.stamp.to_sec())
ros_time.append(t.to_sec())
yaw = [0]
if previous_pcl.size is not 0:
if len(scan_time) == 1:
init_q = get_yaw_quaternion(imu_rot)
prev_q = Quaternion(1, 0, 0, 0)
q_array = [prev_q]
icp = current_pcl.make_IterativeClosestPoint()
# Final = icp.align()
converged, transf, transf_current_pcl, fitness = icp.icp(current_pcl, previous_pcl)
# check if the rotation matrix is valid (i.e. det==1)
if not np.isclose(np.linalg.det(transf[0:3, 0:3]), 1.0):
converged = False
if converged:
time.append(msg.header.stamp.to_sec())
dt = time[-1] - time[-2]
if 0 < dt <= 0.5: #if more than 10 scans (i.e. .5s) have been dropped, do save the pose (same pose as previous)
# get covariance matrix
pose_orientation, pose_translation = matrix4_to_rot_trans(imu_rot, transf, mode='2d')
#pose_orientation, pose_translation, cov = get_cov(transf, current_pcl, transf_current_pcl)
if np.linalg.norm(pose_translation)/dt*3.6 < 40: # the speed between two scans should not be greater than 40 kph
prev_tr = np.append(prev_tr, [prev_q.rotate(pose_translation) + prev_tr[-1]],
axis=0)
prev_q = init_q.conjugate*pose_orientation
q_array.append(prev_q)
scan_time.append(msg.header.stamp.to_sec())
ros_time.append(t.to_sec())
else:
dt = 0
print '%i poses were calculated' % len(scan_time)
print 'Interpolate the poses (orientation, position)'
interp_time = np.linspace(scan_time[0], scan_time[-1], np.round((scan_time[-1] - scan_time[0])*f_pose))
tr = np.zeros((interp_time.size, 3))
interp_ros_time = np.linspace(ros_time[0], ros_time[-1], interp_time.size)
ori = get_interpolated_quat(interp_time, scan_time, q_array)
tr[:, 0] = np.interp(interp_time, scan_time, prev_tr[:, 0])
tr[:, 1] = np.interp(interp_time, scan_time, prev_tr[:, 1])
print 'Change coordinates to footprint_frame'
# rotate from laser_frame to footprint frame
laser_rd = -(90+9)*math.pi/180
X = np.cos(laser_rd) * tr[:, 1] - np.sin(laser_rd) * tr[:, 0]
Y = np.sin(laser_rd) * tr[:, 1] + np.cos(laser_rd) * tr[:, 0]
tr[:, 1] = X
tr[:, 0] = Y
# rotate the orientation
'''rot_ = Quaternion(axis=[0, 0, 1], radians=laser_rd)
ori = [rot_*o for o in ori]'''
print ori
plt.plot(X, Y)
plt.axis('equal')
plt.show()
print 'Writing the poses in the /scan_pose topic'
write_pose_to_bag(bag, interp_time, interp_ros_time, ori, tr)
print 'Re-indexing the bag'
for done in bag.reindex():
pass
bag.close()
print 'Done'
def save_pcl(pcd, name):
pc = pcl.PointCloud()
pc.from_array(pcd.astype(np.float32))
pc.to_file(name)
