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 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 _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 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 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 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 _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)
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)
