def generateBodies(cloud, id): # BEGIN addtoenv names = [] if args.geom_type == "mesh": mesh = generate_mesh(cloud) name = "simple_mesh_%i" % id mesh_body = mk.create_trimesh(env, get_xyz_world_frame(mesh.getCloud()), np.array(mesh.getFaces()), name=name) mesh_body.SetUserData( "bt_use_trimesh", True ) # Tell collision checker to use the trimesh rather than the convex hull of it names.append(name) elif args.geom_type == "cd": big_mesh = generate_mesh(cloud) convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, 30) for (i, mesh) in enumerate(convex_meshes): name = "mesh_%i_%i" % (id, i) verts = get_xyz_world_frame(mesh.getCloud()) mk.create_trimesh(env, verts, mesh.getTriangles(), name=name) randcolor = np.random.rand(3) print 'vertices' print mesh.getVertices() if not isValidMesh(mesh): print('Mesh invalid. Removing kinbody') env.RemoveKinBody(env.GetKinBody(name)) continue env.GetKinBody(name).GetLinks()[0].GetGeometries( )[0].SetAmbientColor(randcolor) env.GetKinBody(name).GetLinks()[0].GetGeometries( )[0].SetDiffuseColor(randcolor) names.append(name) # END addtoenv elif args.geom_type == "spheres": raise Exception("don't use spheres--there's a performance issue") cloud_ds = cloudprocpy.downsampleCloud(cloud, .04) name = "spheres_%i" % id mk.create_spheres(env, get_xyz_world_frame(cloud_ds), .02, name=name) names.append(name) elif args.geom_type == "boxes": cloud_ds = cloudprocpy.downsampleCloud(cloud, .04) name = "boxes_%i" % id mk.create_boxes(env, get_xyz_world_frame(cloud_ds), .02, name=name) names.append(name) return names
def extract_red_alphashape(cloud, robot): """ extract red, get alpha shape, downsample """ raise NotImplementedError # downsample cloud cloud_ds = cloudprocpy.downsampleCloud(cloud, .01) # transform into body frame xyz1_kinect = cloud_ds.to2dArray() xyz1_kinect[:,3] = 1 T_w_k = berkeley_pr2.get_kinect_transform(robot) xyz1_robot = xyz1_kinect.dot(T_w_k.T) # compute 2D alpha shape xyz1_robot_flat = xyz1_robot.copy() xyz1_robot_flat[:,2] = 0 # set z coordinates to zero xyz1_robot_flatalphashape = cloudprocpy.computeAlphaShape(xyz1_robot_flat) # unfortunately pcl alpha shape func throws out the indices, so we have to use nearest neighbor search cloud_robot_flatalphashape = cloudprocpy.CloudXYZ() cloud_robot_flatalphashape.from2dArray(xyz1_robot_flatalphashape) cloud_robot_flat = cloudprocpy.CloudXYZ() cloud_robot_flat.from2dArray(xyz1_robot_flat) alpha_inds = cloudprocpy.getNearestNeighborIndices(xyz1_robot_flatalphashape, xyz1_robot_flat) xyz_robot_alphashape = xyz1_robot_flatalphashape[:,:3] # put back z coordinate xyz_robot_alphashape[:,2] = xyz1_robot[alpha_inds,2] return xyz_robot_alphashape[:,:3]
def downsample(xyz, v): import cloudprocpy if xyz.shape[1] == 3: cloud = cloudprocpy.CloudXYZ() xyz1 = np.ones((len(xyz), 4), 'float') xyz1[:, :3] = xyz cloud.from2dArray(xyz1) cloud = cloudprocpy.downsampleCloud(cloud, v) return cloud.to2dArray()[:, :3] else: # rgb fields needs to be packed and upacked as described in here # http://docs.pointclouds.org/1.7.0/structpcl_1_1_point_x_y_z_r_g_b.html xyzrgb = xyz n = xyzrgb.shape[0] cloud = cloudprocpy.CloudXYZRGB() xyzrgb1 = np.ones((n, 8), 'float') xyzrgb1[:, :3] = xyzrgb[:, :3] xyzrgb1[:, 4] = cloudprocpy.packRGBs(xyzrgb[:, 3:] * 255.0) xyzrgb1[:, 5:] = np.zeros( (n, 3)) # padding that is not used. set to zero just in case cloud.from2dArray(xyzrgb1) cloud = cloudprocpy.downsampleColorCloud(cloud, v) xyzrgb1 = cloud.to2dArray() return np.c_[xyzrgb1[:, :3], cloudprocpy.unpackRGBs(xyzrgb1[:, 4]) / 255.0]
def downsample(xyz, v): cloud = cloudprocpy.CloudXYZ() xyz1 = np.ones((len(xyz),4),'float') xyz1[:,:3] = xyz cloud.from2dArray(xyz1) cloud = cloudprocpy.downsampleCloud(cloud, v) return cloud.to2dArray()[:,:3]
def downsample(xyz, v): cloud = cloudprocpy.CloudXYZ() xyz1 = np.ones((len(xyz), 4), 'float') xyz1[:, :3] = xyz cloud.from2dArray(xyz1) cloud = cloudprocpy.downsampleCloud(cloud, v) return cloud.to2dArray()[:, :3]
def generateBodies(cloud, id): # BEGIN addtoenv names = [] if args.geom_type == "mesh": mesh = generate_mesh(cloud) name = "simple_mesh_%i"%id mesh_body = mk.create_trimesh(env, get_xyz_world_frame(mesh.getCloud()), np.array(mesh.getFaces()), name=name) mesh_body.SetUserData("bt_use_trimesh", True) # Tell collision checker to use the trimesh rather than the convex hull of it names.append(name) elif args.geom_type == "cd": big_mesh = generate_mesh(cloud) convex_meshes = cloudprocpy.convexDecompHACD(big_mesh,30) for (i,mesh) in enumerate(convex_meshes): name = "mesh_%i_%i" % (id,i) verts = get_xyz_world_frame(mesh.getCloud()) mk.create_trimesh(env, verts, mesh.getTriangles(), name=name) randcolor = np.random.rand(3) print 'vertices' print mesh.getVertices() if not isValidMesh(mesh): print('Mesh invalid. Removing kinbody') env.RemoveKinBody(env.GetKinBody(name)) continue env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor) env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor) names.append(name) # END addtoenv elif args.geom_type == "spheres": raise Exception("don't use spheres--there's a performance issue") cloud_ds = cloudprocpy.downsampleCloud(cloud, .04) name = "spheres_%i"%id mk.create_spheres(env, get_xyz_world_frame(cloud_ds), .02, name=name) names.append(name) elif args.geom_type == "boxes": cloud_ds = cloudprocpy.downsampleCloud(cloud, .04) name = "boxes_%i"%id mk.create_boxes(env, get_xyz_world_frame(cloud_ds), .02, name=name) names.append(name) return names
def downsample_colored(xyzrgb, v): import cloudprocpy cloud = cloudprocpy.CloudXYZ() xyz1 = np.ones((len(xyzrgb),4),'float') xyz1[:,:3] = xyzrgb[:,:3] cloud.from2dArray(xyz1) cloud_downsampled = cloudprocpy.downsampleCloud(cloud, v) indices = cloudprocpy.getNearestNeighborIndices(cloud_downsampled, cloud) return np.hstack((cloud_downsampled.to2dArray()[:,:3], xyzrgb[indices,3:]))
def generateBodies(self, cloud, id): # BEGIN addtoenv names = [] if self.geom_type == "mesh": mesh = self.generate_mesh(cloud) name = "simple_mesh_%i"%id mesh_body = mk.create_trimesh(self.env, self.get_xyz_world_frame(mesh.getCloud()), np.array(mesh.getFaces()), name=name) mesh_body.SetUserData("bt_use_trimesh", True) # Tell collision checker to use the trimesh rather than the convex hull of it names.append(name) elif self.geom_type == "cd": big_mesh = self.generate_mesh(cloud) #name = 'big_mesh' #verts = self.get_xyz_world_frame(big_mesh.getCloud()) #mk.create_trimesh(self.env, verts, big_mesh.getTriangles(), name=name) convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, self.num_cd_components) for (i,mesh) in enumerate(convex_meshes): name = "mesh_%i_%i" % (id,i) verts = self.get_xyz_world_frame(mesh.getCloud()) mk.create_trimesh(self.env, verts, mesh.getTriangles(), name=name) randcolor = np.random.rand(3) self.env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor) self.env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor) names.append(name) # END addtoenv elif self.geom_type == "spheres": raise Exception("don't use spheres--there's a performance issue") cloud_ds = cloudprocpy.downsampleCloud(cloud, .04) name = "spheres_%i"%id mk.create_spheres(self.env, self.get_xyz_world_frame(cloud_ds), .02, name=name) names.append(name) elif self.geom_type == "boxes": cloud_ds = cloudprocpy.downsampleCloud(cloud, .04) name = "boxes_%i"%id mk.create_boxes(self.env, self.get_xyz_world_frame(cloud_ds), .02, name=name) names.append(name) return names
def voxelize(self, cloud, voxelSize=0.003): '''TODO''' cloud = numpy.ascontiguousarray( cloud) # cloudprocpy assumes this ordering n = cloud.shape[0] augment = zeros((n, 1)) bigCloud = concatenate((cloud, augment), axis=1) cloudTrajopt = cloudprocpy.CloudXYZ() cloudTrajopt.from2dArray(bigCloud) dsCloud = cloudprocpy.downsampleCloud(cloudTrajopt, voxelSize) cloud = dsCloud.to2dArray()[:, :3] return cloud
def extract_red(rgb, depth, T_w_k): """ extract red points and downsample """ hsv = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV) h = hsv[:,:,0] s = hsv[:,:,1] v = hsv[:,:,2] red_mask = ((h<10) | (h>150)) & (s > 100) & (v > 100) valid_mask = depth > 0 xyz_k = clouds.depth_to_xyz(depth, berkeley_pr2.f) xyz_w = xyz_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:] z = xyz_w[:,:,2] z0 = xyz_k[:,:,2] if DEBUG_PLOTS: cv2.imshow("z0",z0/z0.max()) cv2.imshow("z",z/z.max()) cv2.imshow("rgb", rgb) cv2.waitKey() height_mask = xyz_w[:,:,2] > .7 # XXXX pass in parameter good_mask = red_mask & height_mask & valid_mask if DEBUG_PLOTS: cv2.imshow("red",red_mask.astype('uint8')*255) cv2.imshow("above_table", height_mask.astype('uint8')*255) cv2.imshow("red and above table", good_mask.astype('uint8')*255) print "press enter to continue" cv2.waitKey() good_xyz = xyz_w[good_mask] cloud = cloudprocpy.CloudXYZ() good_xyz1 = np.zeros((len(good_xyz), 4), 'float32') good_xyz1[:,:3] = good_xyz cloud.from2dArray(good_xyz1) cloud_ds = cloudprocpy.downsampleCloud(cloud, .01) return cloud_ds.to2dArray()[:,:3]
def generateBodies(self, cloud, id): # BEGIN addtoenv names = [] if self.geom_type == "mesh": mesh = self.generate_mesh(cloud) name = "simple_mesh_%i"%id mesh_body = mk.create_trimesh(self.env, self.get_xyz_world_frame(mesh.getCloud()), np.array(mesh.getFaces()), name=name) mesh_body.SetUserData("bt_use_trimesh", True) # Tell collision checker to use the trimesh rather than the convex hull of it names.append(name) elif self.geom_type == "cd": big_mesh = self.generate_mesh(cloud) convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, self.num_cd_components) for (i,mesh) in enumerate(convex_meshes): name = "mesh_%i_%i" % (id,i) verts = self.get_xyz_world_frame(mesh.getCloud()) mk.create_trimesh(self.env, verts, mesh.getTriangles(), name=name) randcolor = np.random.rand(3) self.env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor) self.env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor) names.append(name) # END addtoenv elif self.geom_type == "spheres": raise Exception("don't use spheres--there's a performance issue") cloud_ds = cloudprocpy.downsampleCloud(cloud, .04) name = "spheres_%i"%id mk.create_spheres(self.env, self.get_xyz_world_frame(cloud_ds), .02, name=name) names.append(name) elif self.geom_type == "boxes": cloud_ds = cloudprocpy.downsampleCloud(cloud, .04) name = "boxes_%i"%id mk.create_boxes(self.env, self.get_xyz_world_frame(cloud_ds), .02, name=name) names.append(name) return names
def extract_red(rgb, depth, T_w_k): """ extract red points and downsample """ hsv = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV) h = hsv[:, :, 0] s = hsv[:, :, 1] v = hsv[:, :, 2] red_mask = ((h < 10) | (h > 150)) & (s > 100) & (v > 100) valid_mask = depth > 0 xyz_k = clouds.depth_to_xyz(depth, berkeley_pr2.f) xyz_w = xyz_k.dot(T_w_k[:3, :3].T) + T_w_k[:3, 3][None, None, :] z = xyz_w[:, :, 2] z0 = xyz_k[:, :, 2] if DEBUG_PLOTS: cv2.imshow("z0", z0 / z0.max()) cv2.imshow("z", z / z.max()) cv2.imshow("rgb", rgb) cv2.waitKey() height_mask = xyz_w[:, :, 2] > .7 # XXXX pass in parameter good_mask = red_mask & height_mask & valid_mask if DEBUG_PLOTS: cv2.imshow("red", red_mask.astype('uint8') * 255) cv2.imshow("above_table", height_mask.astype('uint8') * 255) cv2.imshow("red and above table", good_mask.astype('uint8') * 255) print "press enter to continue" cv2.waitKey() good_xyz = xyz_w[good_mask] cloud = cloudprocpy.CloudXYZ() good_xyz1 = np.zeros((len(good_xyz), 4), 'float32') good_xyz1[:, :3] = good_xyz cloud.from2dArray(good_xyz1) cloud_ds = cloudprocpy.downsampleCloud(cloud, .01) return cloud_ds.to2dArray()[:, :3]
def add_convexified_pointcloud_to_env(sim, pc2, transform=np.eye(4), num_cd_components=20, point_cloud_filter=lambda point: True): """ Convexifies point cloud and adds to openrave environment :param sim: pr2_sim.simulator.Simulator :param pc2: the point cloud to read from :type pc2: sensor_msgs.PointCloud2 :param transform: 4x4 np.ndarray of transform for cloud in frame base_link :param point_cloud_filter: True if keep point """ transform_world = sim.transform_from_to(transform, 'base_link', 'world') full_cloud = pc2_to_cloudprocpy(pc2, transform_world, point_cloud_filter) cloud = cloudprocpy.downsampleCloud(full_cloud, .005) # .005 big_mesh = generate_mesh(cloud) convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, num_cd_components) for i, mesh in enumerate(convex_meshes): sim.add_kinbody(mesh.getVertices(), mesh.getTriangles(), name='mesh_{0}'.format(i), check_collision=True)
def addCloudToEnvironment(self, cloud, cubeSize=0.02): ''' Add a point cloud to the OpenRAVE environment as a collision obstacle. @param cloud: list of 3-tuples, [(x1,y1,z1),...,(xn,yn,zn)] ''' # remove existing cloud, if there is one self.removeCloudFromEnvironment() # convert point cloud to expected format cloud = ascontiguousarray(cloud) # cloudprocpy assumes this cloud = concatenate((cloud, zeros((cloud.shape[0], 1))), axis=1) cloudTrajopt = cloudprocpy.CloudXYZ() cloudTrajopt.from2dArray(cloud) # downsample and add to environment dsCloud = cloudprocpy.downsampleCloud(cloudTrajopt, cubeSize) self.obstacleCloud = make_kinbodies.create_boxes( self.env, dsCloud.to2dArray()[:, :3], cubeSize / 2.0)
def downsample(xyz, v): import cloudprocpy if xyz.shape[1] == 3: cloud = cloudprocpy.CloudXYZ() xyz1 = np.ones((len(xyz),4),'float') xyz1[:,:3] = xyz cloud.from2dArray(xyz1) cloud = cloudprocpy.downsampleCloud(cloud, v) return cloud.to2dArray()[:,:3] else: # rgb fields needs to be packed and upacked as described in here # http://docs.pointclouds.org/1.7.0/structpcl_1_1_point_x_y_z_r_g_b.html xyzrgb = xyz n = xyzrgb.shape[0] cloud = cloudprocpy.CloudXYZRGB() xyzrgb1 = np.ones((n,8),'float') xyzrgb1[:,:3] = xyzrgb[:,:3] xyzrgb1[:,4] = cloudprocpy.packRGBs(xyzrgb[:,3:] * 255.0) xyzrgb1[:,5:] = np.zeros((n,3)) # padding that is not used. set to zero just in case cloud.from2dArray(xyzrgb1) cloud = cloudprocpy.downsampleColorCloud(cloud, v) xyzrgb1 = cloud.to2dArray() return np.c_[xyzrgb1[:,:3], cloudprocpy.unpackRGBs(xyzrgb1[:,4]) / 255.0]
mesh_body.SetUserData("bt_use_trimesh", True) # Tell collision checker to use the trimesh rather than the convex hull of it elif args.geom_type == "cd": big_mesh = generate_mesh(cloud_orig) convex_meshes = cloudprocpy.convexDecompHACD(big_mesh,30) for (i,mesh) in enumerate(convex_meshes): name = "mesh%i"%i verts = get_xyz_world_frame(mesh.getCloud()) mk.create_trimesh(env, verts, mesh.getTriangles(), name=name) randcolor = np.random.rand(3) env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor) env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor) # END addtoenv elif args.geom_type == "spheres": raise Exception("don't use spheres--there's a performance issue") cloud_nofloor = remove_floor(cloud_orig) cloud_ds = cloudprocpy.downsampleCloud(cloud_nofloor, .04) mk.create_spheres(env, get_xyz_world_frame(cloud_ds), .02) elif args.geom_type == "boxes": cloud_nofloor = remove_floor(cloud_orig) cloud_ds = cloudprocpy.downsampleCloud(cloud_nofloor, .04) mk.create_boxes(env, get_xyz_world_frame(cloud_ds), .02) def full_body_drive_and_reach(robot, link_name, xyz_targ, quat_targ): request = { "basic_info" : { "n_steps" : 10, "manip" : "active", "start_fixed" : True },
big_mesh = generate_mesh(cloud_orig) convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, 30) for (i, mesh) in enumerate(convex_meshes): name = "mesh%i" % i verts = get_xyz_world_frame(mesh.getCloud()) mk.create_trimesh(env, verts, mesh.getTriangles(), name=name) randcolor = np.random.rand(3) env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor( randcolor) env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor( randcolor) # END addtoenv elif args.geom_type == "spheres": raise Exception("don't use spheres--there's a performance issue") cloud_nofloor = remove_floor(cloud_orig) cloud_ds = cloudprocpy.downsampleCloud(cloud_nofloor, .04) mk.create_spheres(env, get_xyz_world_frame(cloud_ds), .02) elif args.geom_type == "boxes": cloud_nofloor = remove_floor(cloud_orig) cloud_ds = cloudprocpy.downsampleCloud(cloud_nofloor, .04) mk.create_boxes(env, get_xyz_world_frame(cloud_ds), .02) def full_body_drive_and_reach(link_name, xyz_targ, quat_targ): request = { "basic_info": { "n_steps": 10, "manip": "active", "start_fixed": True },
import numpy as np import cloudprocpy cloud = cloudprocpy.readPCDXYZ("/home/joschu/Proj/trajoptrave/bigdata/laser_cloud.pcd") cloud = cloudprocpy.downsampleCloud(cloud, .01) cloud = cloudprocpy.boxFilter(cloud, .5,5,-3,3,.1,2.5) cloud.save("/tmp/cloud.ply") xyzorig = cloud.to2dArray()[:,:3] xyzn = cloudprocpy.mlsAddNormals(cloud).to2dArray() xyz = xyzn[:,:3] normals = xyzn[:,4:7] from trajoptpy import math_utils normals = math_utils.normr(normals) camerapos = np.array([0,0,1.3]) ndotp = ((camerapos[None,:] - xyz) * normals).sum(axis=1) normals *= np.sign(ndotp)[:,None] import trajoptpy, openravepy env = openravepy.RaveGetEnvironment(1) if env is None: env = openravepy.Environment() viewer = trajoptpy.GetViewer(env) import IPython IPython.lib.inputhook.set_inputhook(viewer.Step) xyzn[:,4:7] = normals cloudwn = cloudprocpy.CloudXYZN() cloudwn.from2dArray(xyzn) cloudwn.save("/tmp/lasercloud.ply") handles = []