def downsample_random(pc, fraction, random_seed=None): """Randomly downsample pointcloud to a fraction of its size. Returns a pointcloud of size fraction * len(pc), rounded to the nearest integer. Resulting pointcloud has the same SRS and offset as the input. Use random_seed=k for some integer k to get reproducible results. Arguments: pc : pcl.PointCloud Input pointcloud. fraction : float Fraction of points to include. random_seed : int, optional Seed to use in random number generator. Returns: pcl.Pointcloud """ if not 0 < fraction <= 1: raise ValueError("Expected fraction in (0,1], got %r" % fraction) rng = np.random.RandomState(random_seed) k = max(int(round(fraction * len(pc))), 1) sample = rng.choice(len(pc), k, replace=False) new_pc = pc.extract(sample) force_srs(new_pc, same_as=pc) return new_pc
def _load_csv(path, delimiter=','): """ Load a set of points from a CSV file as a pointcloud Returns: pc : pcl.PointCloud """ precise_points = np.genfromtxt(path, delimiter=delimiter, dtype=np.float64) offset = np.mean(precise_points, axis=0, dtype=np.float64) pc = pcl.PointCloud(np.array(precise_points - offset, dtype=np.float32)) force_srs(pc, offset=offset) return pc
def extract_mask(pointcloud, mask): """Extract all points in a mask into a new pointcloud. Arguments: pointcloud : pcl.PointCloud Input pointcloud. mask : numpy.ndarray of bool mask for which points from the pointcloud to include. Returns: pointcloud with the same registration (if any) as the original one.""" pointcloud_new = pointcloud.extract(np.where(mask)[0]) if is_registered(pointcloud): force_srs(pointcloud_new, same_as=pointcloud) return pointcloud_new
def clone(pc): """ Return a copy of a pointcloud, including registration metadata Arguments: pc: pcl.PointCloud() Returns: cp: pcl.PointCloud() """ cp = pcl.PointCloud(np.asarray(pc)) if is_registered(pc): force_srs(cp, same_as=pc) return cp
def test_set_srs(): """Test the set_srs() function""" rdnew = osr.SpatialReference() rdnew.SetFromUserInput("EPSG:28992") latlon = osr.SpatialReference() latlon.SetFromUserInput("EPSG:4326") adam_latlon = np.array( [4.904153991281891, 52.372337993959924, 42.97214563656598], dtype=np.float64) adam_rdnew = np.array([122104.0, 487272.0, 0.0], dtype=np.float64) pcA = pcl.PointCloud([[0., 0., 0.]]) force_srs(pcA, srs=latlon, offset=adam_latlon) pcB = pcl.PointCloud([[0., 0., 0.]]) force_srs(pcB, srs=rdnew, offset=adam_rdnew) # latlon [degrees] -> rdnew [m] set_srs(pcA, same_as=pcB) assert_less( np.max(np.square(np.asarray(pcA))), 1e-3**2, "Coordinate transform not accurate to 1 mm %s" % np.asarray(pcA)) # rdnew [m] -> latlon [degrees] set_srs(pcB, srs=latlon, offset=[0, 0, 0]) # check horizontal error [degrees] error = np.asarray(pcB)[0] - adam_latlon assert_less( np.max(np.square(error[0:2])), (1e-6)**2, "Coordinate transform rdnew->latlon not accurate to 1e-6 degree %s" % error[0:2]) # check vertical error [m] assert_less( abs(error[2]), (1e-6), "Vertical Coordinate in of transform not accurate to 1e-6 meter %s" % abs(error[2]))
def test_set_srs(): """Test the set_srs() function""" rdnew = osr.SpatialReference() rdnew.SetFromUserInput( "EPSG:28992" ) latlon = osr.SpatialReference() latlon.SetFromUserInput( "EPSG:4326" ) adam_latlon = np.array( [4.904153991281891, 52.372337993959924, 42.97214563656598], dtype=np.float64) adam_rdnew = np.array([122104.0, 487272.0, 0.0], dtype=np.float64) pcA = pcl.PointCloud ( [[0.,0.,0.]] ) force_srs( pcA, srs=latlon, offset=adam_latlon ) pcB = pcl.PointCloud ( [[0., 0., 0.]] ) force_srs( pcB, srs=rdnew, offset=adam_rdnew ) # latlon [degrees] -> rdnew [m] set_srs( pcA, same_as=pcB ) assert_less( np.max( np.square( np.asarray( pcA ) ) ), 1e-3 ** 2, "Coordinate transform not accurate to 1 mm %s" % np.asarray(pcA) ) # rdnew [m] -> latlon [degrees] set_srs( pcB, srs=latlon, offset=[0,0,0] ) # check horizontal error [degrees] error = np.asarray(pcB)[0] - adam_latlon assert_less( np.max( np.square(error[0:2]) ), (1e-6) ** 2, "Coordinate transform rdnew->latlon not accurate to 1e-6 degree %s" % error[0:2] ) # check vertical error [m] assert_less( abs(error[2]) , (1e-6) , "Vertical Coordinate in of transform not accurate to 1e-6 meter %s" % abs(error[2]) )
def downsample_voxel(pc, voxel_size=0.01): '''Downsample a pointcloud using a voxel grid filter. Resulting pointcloud has the same SRS and offset as the input. Arguments: pc : pcl.PointCloud Original pointcloud float : voxel_size Grid spacing for the voxel grid Returns: pc : pcl.PointCloud filtered pointcloud ''' pc_filter = pc.make_voxel_grid_filter() pc_filter.set_leaf_size(voxel_size, voxel_size, voxel_size) newpc = pc_filter.filter() force_srs(newpc, same_as=pc) return newpc
def _load_las(lasfile): """Read a LAS file Returns: registered pointcloudxyzrgb The pointcloud has color and XYZ coordinates, and the offset and precision set. """ _check_readable(lasfile) las = None try: las = liblas.file.File(lasfile) lsrs = las.header.get_srs() lsrs = lsrs.get_wkt() n_points = las.header.get_count() precise_points = np.zeros((n_points, 6), dtype=np.float64) for i, point in enumerate(las): precise_points[i] = (point.x, point.y, point.z, point.color.red / 256, point.color.green / 256, point.color.blue / 256) # reduce the offset to decrease floating point errors bbox = BoundingBox(points=precise_points[:, 0:3]) center = bbox.center precise_points[:, 0:3] -= center pointcloud = pcl.PointCloudXYZRGB(precise_points.astype(np.float32)) force_srs(pointcloud, srs=lsrs, offset=center) finally: if las is not None: las.close() return pointcloud
def test_force_srs(): """Test the force_srs() function""" rdnew = osr.SpatialReference() rdnew.SetFromUserInput("EPSG:28992") latlon = osr.SpatialReference() latlon.SetFromUserInput("EPSG:4326") # using offset and srs (text) pcA = pcl.PointCloud([[1, 2, 3]]) force_srs(pcA, offset=[0, 0, 0], srs="EPSG:28992") assert_true(is_registered(pcA)) assert_array_almost_equal(pcA.offset, np.zeros(3, dtype=np.float64), 15, "Offset not zero to 15 decimals") assert_true(pcA.srs.IsSame(rdnew)) # using same_as pcB = pcl.PointCloud([[1, 2, 3]]) force_srs(pcB, same_as=pcA) assert_true(is_registered(pcB)) assert_array_almost_equal(pcB.offset, np.zeros(3, dtype=np.float64), 15, "Offset not zero to 15 decimals") assert_true(pcB.srs.IsSame(rdnew)) # using no offset and osr.SpatialReference() pcC = pcl.PointCloud([[1, 2, 3]]) force_srs(pcC, srs=rdnew) assert_true(pcC.srs.IsSame(rdnew)) assert_false(hasattr(pcC, "offset")) # testing if no actual transform occurs on the points force_srs(pcC, srs=latlon) assert_false(pcC.srs.IsSame(pcA.srs)) assert_array_almost_equal(np.asarray(pcA), np.asarray(pcC), 8, "force_srs() should not alter points")
def test_force_srs(): """Test the force_srs() function""" rdnew = osr.SpatialReference() rdnew.SetFromUserInput( "EPSG:28992" ) latlon = osr.SpatialReference() latlon.SetFromUserInput( "EPSG:4326" ) # using offset and srs (text) pcA = pcl.PointCloud( [[1,2,3]] ) force_srs( pcA, offset=[0,0,0], srs="EPSG:28992" ) assert_true( is_registered( pcA ) ) assert_array_almost_equal( pcA.offset, np.zeros(3, dtype=np.float64), 15, "Offset not zero to 15 decimals" ) assert_true( pcA.srs.IsSame( rdnew ) ) # using same_as pcB = pcl.PointCloud( [[1,2,3]] ) force_srs( pcB, same_as=pcA ) assert_true( is_registered( pcB ) ) assert_array_almost_equal( pcB.offset, np.zeros(3, dtype=np.float64), 15, "Offset not zero to 15 decimals" ) assert_true( pcB.srs.IsSame( rdnew ) ) # using no offset and osr.SpatialReference() pcC = pcl.PointCloud( [[1,2,3]] ) force_srs( pcC, srs=rdnew ) assert_true( pcC.srs.IsSame( rdnew ) ) assert_false( hasattr( pcC, "offset" ) ) # testing if no actual transform occurs on the points force_srs(pcC, srs=latlon ) assert_false( pcC.srs.IsSame( pcA.srs ) ) assert_array_almost_equal( np.asarray(pcA), np.asarray(pcC), 8, "force_srs() should not alter points" )
except: Initial_scale = None assert os.path.exists(sourcefile), sourcefile + ' does not exist' assert os.path.exists(drivemapfile), drivemapfile + ' does not exist' assert os.path.exists(footprintcsv), footprintcsv + ' does not exist' ##### # Setup * the low-res drivemap # * footprint # * pointcloud # * up-vector log("Reading drivemap", drivemapfile) drivemap = load(drivemapfile) force_srs(drivemap, srs="EPSG:32633") log("Reading footprint", footprintcsv) footprint = load(footprintcsv) force_srs(footprint, srs="EPSG:32633") set_srs(footprint, same_as=drivemap) log("Reading object", sourcefile) pointcloud = load(sourcefile) Up = None try: with open(up_file) as f: dic = json.load(f) Up = np.array(dic['estimatedUpDirection']) log("Reading up_file", up_file)