wassname/pyrobolearn
1
0
Fork 0
mirror of https://github.com/wassname/pyrobolearn.git synced 2026-09-09 11:31:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

update world: update heightmap and terrain generators

Browse Source
This commit is contained in:
Brian Delhaisse
2019-05-01 05:50:54 +02:00
parent 0033bf0104
commit dddc8d7a47
18 changed files with 412550 additions and 957 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pyrobolearn/worlds/utils/heightmap_generator.py
-434
View File
@@ -1,434 +0,0 @@
#!/usr/bin/env python
"""Provide heightmap generators.
The various functions defined here generate
"""
import numpy as np
from sklearn.gaussian_process import GaussianProcessRegressor
from sklearn.gaussian_process.kernels import RBF
from scipy.interpolate import Rbf
try:
import gdal
except ImportError as e:
raise ImportError(repr(e) + '\nTry to install gdal: pip install gdal')
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
def diamond_square_algorithm(n=8, init_values=None, noise=0, lower_bound=0, upper_bound=255, dtype=np.int, seed=None):
r"""Diamond-Square Algorithm
This function implements the diamond-square algorithm [1], to generate random terrains given an initial value
for each corner.
Warnings: the diamond-square algo assumes that the heightmap is a 2D square array.
Args:
n (int): number of points (must be a power of 2). From this, the width and the height will automatically be
computed, such that width = height = 2**n + 1.
init_values (np.array[4], None): the four initial values for the corners. If None, it will generate 4 values
randomly such that they are between the lower_bound and upper_bound.
noise (int,float): noise level to add. This corresponds to the standard deviation of the normal distribution.
lower_bound (int,float): lower bound; each value in the heightmap will be higher than or equal to this bound
upper_bound (int,float): upper bound; each value in the heightmap will be lower than or equal to this bound
dtype (np.int, np.float): type of the returned array for the heightmap
seed (int, None): random seed
Returns:
np.array[2**n+1, 2**n+1]: resulting 2D square heightmap
References:
[1] Wikipedia: https://en.wikipedia.org/wiki/Diamond-square_algorithm
[2] https://blog.habrador.com/2013/02/how-to-generate-random-terrain.html
"""
# set the seed if given
if seed:
np.random.seed(seed)
# create initial heightmap
width, height = 2**n + 1, 2**n + 1
heightmap = -1 * np.ones((height, width), dtype=dtype)
if not init_values:
if dtype == np.int:
init_values = np.random.randint(low=lower_bound, high=upper_bound+1, size=4)
else:
init_values = np.random.uniform(low=lower_bound, high=upper_bound, size=4)
heightmap[0, 0], heightmap[0, width - 1], heightmap[height - 1, 0], heightmap[height - 1, width - 1] = init_values
# define diamond-square step function
def diamond_square_step(heightmap, square=None, noise=0, lower_bound=0, upper_bound=255):
"""
Diamond-square step which which performs a diamond step followed by a square step.
Args:
heightmap (np.array[2*N+1,2*N+1]): heightmap (initial square)
square (np.array[M,M]): the current square we focus on.
"""
# if no square given
if square is None:
height, width = heightmap.shape
square = np.array([[0, 0], [width - 1, 0], [width - 1, height - 1], [0, height - 1]])
# check size of square
xmin, xmax, ymin, ymax = square[:, 0].min(), square[:, 0].max(), square[:, 1].min(), square[:, 1].max()
dx, dy = (xmax - xmin), (ymax - ymin)
if dx == 0 or dx == 1 or dy == 0 or dy == 1:
return
# DIAMOND STEP
center = np.array([xmin + dx / 2, ymin + dy / 2])
yc, xc = center
heightmap[xc, yc] = np.mean([heightmap[x, y] for (y, x) in square]) # + np.random.normal(scale=noise)
heightmap[xc, yc] = min(max(lower_bound, heightmap[xc, yc]), upper_bound) # lower and upper bound
# SQUARE STEP
# triangles: a triangle is defined by 3 points
triangles = np.array([[c1, c2, center] for c1, c2 in zip(square, list(square[1:]) + [square[0]])])
squares = []
for i, triangle in enumerate(triangles):
xmin, xmax, ymin, ymax = triangle[:, 0].min(), triangle[:, 0].max(), triangle[:, 1].min(), triangle[:,
1].max()
if i == 0: # upper triangle
center = np.array([xmin + (xmax - xmin) / 2, ymin])
square = np.array([[xmin, ymin], center, [center[0], ymax], [xmin, ymax]]) # left upper square
elif i == 1: # right triangle
center = np.array([xmax, ymin + (ymax - ymin) / 2])
square = np.array([[xmin, ymin], [xmax, ymin], center, [xmin, center[1]]]) # right upper square
elif i == 2: # lower triangle
center = np.array([xmin + (xmax - xmin) / 2, ymax])
square = np.array([[center[0], ymin], [xmax, ymin], [xmax, ymax], center]) # right lower square
else: # left triangle
center = np.array([xmin, ymin + (ymax - ymin) / 2])
square = np.array([center, [xmax, center[1]], [xmax, ymax], [xmin, ymax]]) # left lower square
yc, xc = center
heightmap[xc, yc] = np.mean([heightmap[x, y] for (y, x) in triangle]) # + np.random.normal(scale=noise)
heightmap[xc, yc] = min(max(lower_bound, heightmap[xc, yc]), upper_bound) # lower and upper bound
# a square is defined by 4 points
squares.append(square)
# for each subsquare in the original square, compute the heightmap recursively
for square in squares:
diamond_square_step(heightmap, square, noise, lower_bound, upper_bound)
# start diamond-square algorithm (recursively)
diamond_square_step(heightmap, noise=noise, lower_bound=lower_bound, upper_bound=upper_bound)
return heightmap
def heightmap_gpr(init_values, x, y, kernel=None, alpha=1e-10, lower_bound=0, upper_bound=255, dtype=np.int):
r"""
Generate a heightmap using gaussian process regression. The advantages of using this method over others to
generate terrains lies in the capacity of adding prior knowledge through the kernel and the given initial values.
For instance, using a RBF kernel means that we want a smooth terrain instead of a bumpy one.
Furthermore, it allows to generate heightmaps which are not necessary square; i.e. they can be rectangular.
Warnings: this is pretty difficult to exploit if the given data is not consistent. See `heigthmap_rbf` for
a better way to generate heightmap.
Args:
init_values (np.array[M,3]): list of `M` 3D points which corresponds to initial values that are used to fit
the gaussian process.
x (np.array[N], np.array[N,O]): If 1d array, it will compute the meshgrid. Otherwise, the resulting 2D array
from the meshgrid is expected. This is used to predict the heightmap at the given points.
y (np.array[0], np.array[N,O]): If 1d array, it will compute the meshgrid. Otherwise, the resulting 2D array
from the meshgrid is expected. This is used to predict the heightmap at the given points.
kernel (None, sklearn.gaussian_process.kernels.Kernel): "The kernel specifying the covariance function of
the GP. If None is passed, the kernel '1.0 * RBF(1.0)' is used as default. Note that the kernel's
hyperparameters are optimized during fitting" [2]
alpha (float, array_like): "Value added to the diagonal of the kernel matrix during fitting. Larger values
correspond to increased noise level in the observations. This can also prevent a potential numerical issue
during fitting, by ensuring that the calculated values form a positive definite matrix. If an array is
passed, it must have the same number of entries as the data used for fitting and is used as
datapoint-dependent noise level. Note that this is equivalent to adding a WhiteKernel with c=alpha.
Allowing to specify the noise level directly as a parameter is mainly for convenience and for consistency
with Ridge." [2]
lower_bound (int,float): lower bound; each value in the heightmap will be higher than or equal to this bound
upper_bound (int,float): upper bound; each value in the heightmap will be lower than or equal to this bound
dtype (np.int, np.float): type of the returned array for the heightmap
Returns:
np.array[N,O]: resulting 2D heightmap
References:
[1] "Gaussian Processes for Machine Learning", Rasmussen and Williams, 2006
[2] Sklearn: https://scikit-learn.org/stable/modules/gaussian_process.html
"""
# check given x and y
if len(x.shape) == 1 and len(y.shape) == 1:
x, y = np.meshgrid(x, y)
if x.shape != y.shape:
raise ValueError("Expecting x and y to have the same shape, which should be the case if it is a meshgrid")
# compute the minimum distance between points
N = len(init_values)
min_dist = np.inf
for i in range(N):
for j in range(i+1, N):
dist = np.linalg.norm(init_values[i, :2] - init_values[j, :2])
if dist < min_dist:
min_dist = dist
print("Min dist: {}".format(min_dist))
# check initial values
if not isinstance(init_values, np.ndarray):
raise TypeError("Expecting init_values to be a numpy array")
if init_values.shape[1] != 3:
raise ValueError("Expecting a numpy array of 3D points for init_values")
# create gaussian process and fit on the given initial values
kernel = RBF(length_scale=np.sqrt(min_dist))
gpr = GaussianProcessRegressor(kernel=kernel, alpha=alpha, normalize_y=True)
gpr.fit(init_values[:, :2], init_values[:, 2])
# predict the heightmap using GPR
X = np.dstack((x, y)).reshape(-1, 2)
heightmap = gpr.predict(X)
heightmap = heightmap.reshape(x.shape)
print("Params: {}".format(gpr.get_params()))
# make sure the values of the heightmap are between the bounds (in-place), and is the correct type
np.clip(heightmap, lower_bound, upper_bound, heightmap)
heightmap.astype(dtype)
return heightmap
def heightmap_rbf(init_values, x, y, function='multiquadric', lower_bound=0, upper_bound=255, dtype=np.int):
r"""
Generate heightmap by interpolating the given initial points using RBF functions.
Advantages: fast and easy to use, and the results are pretty good. Heightmaps can also be rectangular.
Args:
init_values (np.array[M,3]): list of `M` 3D points which corresponds to initial values that are used to fit
the gaussian process.
x (np.array[N], np.array[N,O]): If 1d array, it will compute the meshgrid. Otherwise, the resulting 2D array
from the meshgrid is expected. This is used to predict the heightmap at the given points.
y (np.array[0], np.array[N,O]): If 1d array, it will compute the meshgrid. Otherwise, the resulting 2D array
from the meshgrid is expected. This is used to predict the heightmap at the given points.
function (str, callable): "The radial basis function, based on the radius, r, given by the norm
(default is Euclidean distance);
'multiquadric': sqrt((r/self.epsilon)**2 + 1)
'inverse': 1.0/sqrt((r/self.epsilon)**2 + 1)
'gaussian': exp(-(r/self.epsilon)**2)
'linear': r
'cubic': r**3
'quintic': r**5
'thin_plate': r**2 * log(r)
If callable, then it must take 2 arguments (self, r). The epsilon parameter will be available as
self.epsilon. Other keyword arguments passed in will be available as well." [1]
lower_bound (int,float): lower bound; each value in the heightmap will be higher than or equal to this bound
upper_bound (int,float): upper bound; each value in the heightmap will be lower than or equal to this bound
dtype (np.int, np.float): type of the returned array for the heightmap
Returns:
np.array[N,O]: resulting 2D heightmap
References:
[1] https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.Rbf.html
"""
# check given x and y
if len(x.shape) == 1 and len(y.shape) == 1:
x, y = np.meshgrid(x, y)
if x.shape != y.shape:
raise ValueError("Expecting x and y to have the same shape, which should be the case if it is a meshgrid")
origin_shape = x.shape
rbf = Rbf(init_values[:, 0], init_values[:, 1], init_values[:, 2], function=function)
heightmap = rbf(x.reshape(-1), y.reshape(-1))
heightmap = heightmap.reshape(origin_shape)
# make sure the values of the heightmap are between the bounds (in-place), and is the correct type
np.clip(heightmap, lower_bound, upper_bound, heightmap)
heightmap.astype(dtype)
return heightmap
def heightmap_equation(x, y, z, lower_bound=0, upper_bound=255, dtype=np.int):
r"""
Generate heightmap from 3D equation :math:`z = f(x,y)`.
Args:
x (np.array[N], np.array[N,O]): If 1d array, it will compute the meshgrid. Otherwise, the resulting 2D array
from the meshgrid is expected. This is used to predict the heightmap at the given points.
y (np.array[0], np.array[N,O]): If 1d array, it will compute the meshgrid. Otherwise, the resulting 2D array
from the meshgrid is expected. This is used to predict the heightmap at the given points.
z (callable): it must be a function that accepts two arguments `x` and `y` which will be the arrays from the
meshgrid.
lower_bound (int,float): lower bound; each value in the heightmap will be higher than or equal to this bound
upper_bound (int,float): upper bound; each value in the heightmap will be lower than or equal to this bound
dtype (np.int, np.float): type of the returned array for the heightmap
Examples of 2D surfaces:
z = lambda x,y: np.log(y)
z = lambda x,y: np.sin(np.pi * x) * np.sin(np.pi * y)
Returns:
np.array[N,O]: resulting 2D heightmap
"""
# check given x and y
if len(x.shape) == 1 and len(y.shape) == 1:
x, y = np.meshgrid(x, y)
if x.shape != y.shape:
raise ValueError("Expecting x and y to have the same shape, which should be the case if it is a meshgrid")
origin_shape = x.shape
# call z function: z=f(x,y)
heightmap = z(x, y)
# make sure the values of the heightmap are between the bounds (in-place), and is the correct type
np.clip(heightmap, lower_bound, upper_bound, heightmap)
heightmap.astype(dtype)
return heightmap
def heightmap_gdal(filename, subsample=None, interpolate_fct='multiquadric', lower_bound=0, upper_bound=255,
dtype=np.int):
r"""
Heightmap generated using the Geospatial Data Abstraction Library (GDAL), which allows to open Digital Elevation
Models (DEM) or Geographic Information System (GIS). It can open a .tiff, .geotiff, ascii grid, or
image (jpg, png,...) file.
Args:
filename (str): path to a DEM, GIS, or image file
subsample (int, None): if not None, it is the number of points to sub-sample (to smooth the heightmap using
the specified function)
interpolate_fct (str, callable): "The radial basis function, based on the radius, r, given by the norm
(default is Euclidean distance);
'multiquadric': sqrt((r/self.epsilon)**2 + 1)
'inverse': 1.0/sqrt((r/self.epsilon)**2 + 1)
'gaussian': exp(-(r/self.epsilon)**2)
'linear': r
'cubic': r**3
'quintic': r**5
'thin_plate': r**2 * log(r)
If callable, then it must take 2 arguments (self, r). The epsilon parameter will be available as
self.epsilon. Other keyword arguments passed in will be available as well." [1]
lower_bound (int,float): lower bound; each value in the heightmap will be higher than or equal to this bound
upper_bound (int,float): upper bound; each value in the heightmap will be lower than or equal to this bound
dtype (np.int, np.float): type of the returned array for the heightmap
Returns:
np.array[H,W]: resulting 2D array of size width `W` and height `H`
References:
[1] https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.Rbf.html
"""
# load data (raster)
data = gdal.Open(filename)
band = data.GetRasterBand(1)
heightmap = band.ReadAsArray() # elevation values
if isinstance(subsample, int) and subsample > 0:
height, width = heightmap.shape
idx_x = np.linspace(0, height-1, subsample, dtype=np.int)
idx_y = np.linspace(0, width-1, subsample, dtype=np.int)
idx_x, idx_y = np.meshgrid(idx_x, idx_y)
x, y = np.arange(width), np.arange(height)
x, y = np.meshgrid(x, y)
rbf = Rbf(x[idx_x, idx_y], y[idx_x, idx_y], heightmap[idx_x, idx_y], function=interpolate_fct)
# Nx, Ny = x.shape[0] / subsample, x.shape[1] / subsample
# rbf = Rbf(x[::Nx, ::Ny], y[::Nx, ::Ny], heightmap[::Nx, ::Ny], function=interpolate_fct)
heightmap = rbf(x, y)
# make sure the values of the heightmap are between the bounds (in-place), and is the correct type
if lower_bound and upper_bound:
np.clip(heightmap, lower_bound, upper_bound, heightmap)
elif lower_bound:
np.clip(heightmap, lower_bound, heightmap.max(), heightmap)
elif upper_bound:
np.clip(heightmap, heightmap.min(), upper_bound, heightmap)
if dtype:
heightmap.astype(dtype)
return heightmap
# alias
heigtmap_from_image = heightmap_gdal
# Tests
# Conclusion: use `heightmap_rbf` or `heightmap_gdal` as it is pretty good
if __name__ == '__main__':
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
# define plot figure for heightmap
def plot_figure(heightmap, title='', block=True, z_upper_lim=256):
fig = plt.figure()
fig.suptitle(title)
# 1st subplot: 2D heightmap
ax = fig.add_subplot(1, 2, 1)
ax.set_title('2D heightmap')
ax.imshow(heightmap, cmap='gray')
# 2nd subplot: associated 3D terrain
ax = fig.add_subplot(1, 2, 2, projection='3d')
ax.set_title('3D terrain')
x = np.linspace(0, 1, heightmap.shape[0])
y = np.linspace(0, 1, heightmap.shape[1])
x, y = np.meshgrid(y, x)
ax.plot_surface(x, y, heightmap)
ax.set_zlim(0, z_upper_lim)
print(x.shape)
plt.show(block=block)
# # generate heightmap using the diamond-square algorithm
# N = 8 # shape of map: 2**N+1, 2**N+1
# heightmap = diamond_square_algorithm(N)
# plot_figure(heightmap, title='Diamond-Square algorithm')
# # generate heightmap using gaussian process regression
# x = np.array(range(256))
# y = np.array(range(256))
# N_init = 20
# x_init = np.random.randint(low=x.min(), high=x.max(), size=N_init)
# y_init = np.random.randint(low=y.min(), high=y.max(), size=N_init)
# z_init = np.random.randint(low=0, high=20, size=N_init)
# init_values = np.vstack((x_init, y_init, z_init)).T # shape: Nx3
# #init_values = np.array([[163, 73, 0], [13, 15, 1],[69, 102, 2]])
# #init_values = np.array([[182, 48, 89], [182, 20, 150], [167, 247, 131]])
# heightmap = heightmap_gpr(init_values=init_values, x=x, y=y)
# plot_figure(heightmap, title='Gaussian Process Regression')
# generate heightmap using RBF interpolations
x = np.array(range(256))
y = np.array(range(256)) # range(128)
N_init = 20 # number of bumps
x_init = np.random.randint(low=x.min(), high=x.max(), size=N_init)
y_init = np.random.randint(low=y.min(), high=y.max(), size=N_init)
z_init = np.random.randint(low=0, high=20, size=N_init)
init_values = np.vstack((x_init, y_init, z_init)).T # shape: Nx3
# init_values = np.array([[211, 184, 3], [97, 59, 4], [37, 179, 8], [168, 32, 8], [198, 74, 13],
# [44, 10, 2], [175, 102, 6], [6, 22, 1], [35, 165, 6], [169, 211, 16],
# [158, 119, 18], [228, 63, 13], [40, 62, 15], [76, 221, 10], [1, 113, 10],
# [178, 194, 2], [23, 176,10], [231, 88, 7], [247, 209, 6], [72, 94, 2]])
heightmap = heightmap_rbf(init_values=init_values, x=x, y=y, function='gaussian') # 'linear', 'multiquadric'
plot_figure(heightmap, title='RBF interpolation')
# # generate heigthmap from an image or tif file
# dem = heightmap_gdal('../tests/canyon-geo.tif')
# dem = heightmap_gdal('../tests/dem.jpg')
dem = heightmap_gdal('../tests/heightmap.png')
plot_figure(dem, block=True)
pyrobolearn/worlds/utils/heightmaps/__init__.py
+1
View File
@@ -0,0 +1 @@
# use `diamond_square`, `rbf`, or `geospatial` as they are pretty good
pyrobolearn/worlds/utils/heightmaps/diamond_square.py
+223
View File
@@ -0,0 +1,223 @@
#!/usr/bin/env python
"""Provide the diamond-square heightmap generator.
Generate the heightmap using the diamond-square algorithm [1].
References:
[1] Wikipedia: https://en.wikipedia.org/wiki/Diamond-square_algorithm
"""
import numpy as np
__author__ = ["Brian Delhaisse", "Jamie Scollay"]
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse", "Jamie Scollay"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
# the `diamond_square_heightmap_2` was originally written by Jamie Scollay
# it was then reviewed by Brian Delhaisse, notably with respect to the original code:
# - it has been cleaned; removed all the ";"
# - it has been optimized:
# - it now uses numpy instead of math and lists, it uses `list.append` instead of adding strings, and then `join`
# - it has been simplified.
# - comments have been added and a better documentation is provided
def diamond_square_heightmap(n=8, min_height=0, max_height=255, noise=0, noise_factor=1, init_values=None,
dtype=np.int, seed=None):
r"""Diamond-Square Algorithm
This function implements the diamond-square algorithm [1], to generate random terrains given an initial value
for each corner.
Warnings: the diamond-square algo assumes that the heightmap is a 2D square array.
Args:
n (int): used to create a square array of width and height of 2**n + 1. It also specifies the number of
diamond and square steps.
min_height (int,float): lower bound; each value in the heightmap will be higher than or equal to this bound
max_height (int,float): upper bound; each value in the heightmap will be lower than or equal to this bound
noise (int, float): noise level to add. This corresponds to the standard deviation of the normal distribution.
noise_factor (int, float): after each step, the noise is divided by the given factor.
init_values (np.array[4], None): the four initial values for the corners. If None, it will generate 4 values
randomly such that they are between the min_height and max_height.
dtype (np.int, np.float): type of the returned array for the heightmap
seed (int, None): random seed
Returns:
np.array[2**n+1, 2**n+1]: resulting 2D square heightmap
References:
[1] Wikipedia: https://en.wikipedia.org/wiki/Diamond-square_algorithm
[2] https://blog.habrador.com/2013/02/how-to-generate-random-terrain.html
"""
# set the seed if given
if seed:
np.random.seed(seed)
# create initial heightmap
width, height = 2**n + 1, 2**n + 1
heightmap = -1 * np.ones((height, width), dtype=dtype)
if not init_values:
if dtype == np.int:
init_values = np.random.randint(low=min_height, high=max_height + 1, size=4)
else:
init_values = np.random.uniform(low=min_height, high=max_height, size=4)
heightmap[0, 0], heightmap[0, width - 1], heightmap[height - 1, 0], heightmap[height - 1, width - 1] = init_values
# define diamond-square step function
def diamond_square_step(heightmap, square=None, noise=0, min_height=0, max_height=255):
"""
Diamond-square step which which performs a diamond step followed by a square step.
Args:
heightmap (np.array[2*N+1,2*N+1]): heightmap (initial square)
square (np.array[M,M]): the current square we focus on.
"""
# if no square given
if square is None:
height, width = heightmap.shape
square = np.array([[0, 0], [width - 1, 0], [width - 1, height - 1], [0, height - 1]])
# check size of square
xmin, xmax, ymin, ymax = square[:, 0].min(), square[:, 0].max(), square[:, 1].min(), square[:, 1].max()
dx, dy = (xmax - xmin), (ymax - ymin)
if dx == 0 or dx == 1 or dy == 0 or dy == 1:
return
# DIAMOND STEP
center = np.array([xmin + dx / 2, ymin + dy / 2])
yc, xc = center
heightmap[xc, yc] = np.mean([heightmap[x, y] for (y, x) in square]) # + np.random.normal(scale=noise)
heightmap[xc, yc] = min(max(min_height, heightmap[xc, yc]), max_height) # lower and upper bound
# SQUARE STEP
# triangles: a triangle is defined by 3 points
triangles = np.array([[c1, c2, center] for c1, c2 in zip(square, list(square[1:]) + [square[0]])])
squares = []
for i, triangle in enumerate(triangles):
xmin, xmax, ymin, ymax = triangle[:, 0].min(), triangle[:, 0].max(), \
triangle[:, 1].min(), triangle[:, 1].max()
if i == 0: # upper triangle
center = np.array([xmin + (xmax - xmin) / 2, ymin])
square = np.array([[xmin, ymin], center, [center[0], ymax], [xmin, ymax]]) # left upper square
elif i == 1: # right triangle
center = np.array([xmax, ymin + (ymax - ymin) / 2])
square = np.array([[xmin, ymin], [xmax, ymin], center, [xmin, center[1]]]) # right upper square
elif i == 2: # lower triangle
center = np.array([xmin + (xmax - xmin) / 2, ymax])
square = np.array([[center[0], ymin], [xmax, ymin], [xmax, ymax], center]) # right lower square
else: # left triangle
center = np.array([xmin, ymin + (ymax - ymin) / 2])
square = np.array([center, [xmax, center[1]], [xmax, ymax], [xmin, ymax]]) # left lower square
yc, xc = center
heightmap[xc, yc] = np.mean([heightmap[x, y] for (y, x) in triangle]) # + np.random.normal(scale=noise)
heightmap[xc, yc] = min(max(min_height, heightmap[xc, yc]), max_height) # lower and upper bound
# a square is defined by 4 points
squares.append(square)
# for each subsquare in the original square, compute the heightmap recursively
for square in squares:
diamond_square_step(heightmap, square, noise / noise_factor, min_height, max_height)
# start diamond-square algorithm (recursively)
diamond_square_step(heightmap, noise=noise, min_height=min_height, max_height=max_height)
return heightmap
def diamond_square_heightmap_2(n=8, min_height=0, max_height=255, noise=0, noise_factor=1):
"""
Create a 2D square heightmap using the diamond square algorithm [1]
Args:
n (int): used to create a square array of width and height of 2**n + 1. It also specifies the number of
diamond and square steps.
min_height (float): minimum height
max_height (float): maximum height
noise (float): magnitude of the noise added to the computed height.
noise_factor (float): after each step, the jitter is divided by the given factor.
Returns:
np.array[size, size]: 2D square heightmap
References:
[1] Wikipedia: https://en.wikipedia.org/wiki/Diamond-square_algorithm
"""
# compute the width and height size (i.e. size of the 2D square array/heightmap)
size = int(2 ** n + 1)
# create initial heightmap of width and height size
heightmap = np.zeros((size, size))
# assign a random height at each corner of the heightmap
heightmap[0, 0] = np.random.rand() * max_height
heightmap[0, size - 1] = np.random.rand() * max_height
heightmap[size - 1, 0] = np.random.rand() * max_height
heightmap[size - 1, size - 1] = np.random.rand() * max_height
# for each diamond and square step
for i in range(n):
stride = int((size - 1) / 2 ** (i + 1))
radius = int((size - 1) / 2 ** i)
for j in range(2 ** i):
for k in range(2 ** i):
height = (heightmap[j * radius, k * radius] + heightmap[2 * stride + j * radius, k * radius] +
heightmap[j * radius, 2 * stride + k * radius] +
heightmap[2 * stride + j * radius, 2 * stride + k * radius]) / 4. + (
np.random.rand() - 0.5) * noise
heightmap[stride + j * radius, stride + k * radius] = height
for j in range(2 ** (i + 1) + 1):
for k in range(2 ** i + j % 2):
cnt = 0
if j == 0:
height1 = 0
else:
height1 = heightmap[(j - 1) * stride, stride * ((j + 1) % 2) + k * radius]
cnt += 1
if k == 0 and j % 2 == 1:
height4 = 0
else:
height4 = heightmap[j * stride, stride * (((j + 1) % 2) - 1) + k * radius]
cnt += 1
if j == 2 ** (i + 1):
height3 = 0
else:
height3 = heightmap[(j + 1) * stride, stride * ((j + 1) % 2) + k * radius]
cnt += 1
if k == (2 ** i + j % 2 - 1) and j % 2 == 1:
height2 = 0
else:
height2 = heightmap[j * stride, stride * (((j + 1) % 2) + 1) + k * radius]
cnt += 1
height = float(height1 + height2 + height3 + height4) / cnt + (np.random.rand() - 0.5) * noise
heightmap[j * stride, ((j + 1) % 2) * stride + k * radius] = height
noise /= noise_factor
lowest_point = heightmap.min()
highest_point = heightmap.max()
if n > 1:
for i in range(size):
for j in range(4):
heightmap[j, i] = lowest_point + j * 0.25 * (heightmap[j, i] - lowest_point)
heightmap[size - j - 1, i] = lowest_point + j * 0.25 * (heightmap[size - j - 1, i] - lowest_point)
heightmap[i, j] = lowest_point + j * 0.25 * (heightmap[i, j] - lowest_point)
heightmap[i, size - j - 1] = lowest_point + j * 0.25 * (heightmap[i, size - j - 1] - lowest_point)
return heightmap
pyrobolearn/worlds/utils/heightmaps/equation.py
+56
View File
@@ -0,0 +1,56 @@
#!/usr/bin/env python
r"""Provide the equation heightmap generator.
Generate a heightmap from a 3D equation :math:`z = f(x, y)`.
"""
import numpy as np
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
def equation_heightmap(x, y, z, min_height=0, max_height=255, dtype=np.int):
r"""
Generate heightmap from 3D equation :math:`z = f(x,y)`.
Args:
x (np.array[N], np.array[N,O]): If 1d array, it will compute the meshgrid. Otherwise, the resulting 2D array
from the meshgrid is expected. This is used to predict the heightmap at the given points.
y (np.array[0], np.array[N,O]): If 1d array, it will compute the meshgrid. Otherwise, the resulting 2D array
from the meshgrid is expected. This is used to predict the heightmap at the given points.
z (callable): it must be a function that accepts two arguments `x` and `y` which will be the arrays from the
meshgrid.
min_height (int,float): lower bound; each value in the heightmap will be higher than or equal to this bound
max_height (int,float): upper bound; each value in the heightmap will be lower than or equal to this bound
dtype (np.int, np.float): type of the returned array for the heightmap
Examples of 2D surfaces:
z = lambda x,y: np.log(y)
z = lambda x,y: np.sin(np.pi * x) * np.sin(np.pi * y)
Returns:
np.array[N,O]: resulting 2D heightmap
"""
# check given x and y
if len(x.shape) == 1 and len(y.shape) == 1:
x, y = np.meshgrid(x, y)
if x.shape != y.shape:
raise ValueError("Expecting x and y to have the same shape, which should be the case if it is a meshgrid")
origin_shape = x.shape
# call z function: z=f(x,y)
heightmap = z(x, y)
# make sure the values of the heightmap are between the bounds (in-place), and is the correct type
np.clip(heightmap, min_height, max_height, heightmap)
heightmap.astype(dtype)
return heightmap
pyrobolearn/worlds/utils/heightmaps/geospatial.py
+91
View File
@@ -0,0 +1,91 @@
#!/usr/bin/env python
"""Provide heightmap generators.
The various functions defined here generate
"""
import numpy as np
from scipy.interpolate import Rbf
try:
import gdal
except ImportError as e:
raise ImportError(repr(e) + '\nTry to install gdal: pip install gdal')
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
def gdal_heightmap(filename, subsample=None, interpolate_fct='multiquadric', min_height=0, max_height=255,
dtype=np.int):
r"""
Heightmap generated using the Geospatial Data Abstraction Library (GDAL), which allows to open Digital Elevation
Models (DEM) or Geographic Information System (GIS). It can open a .tiff, .geotiff, ascii grid, or
image (jpg, png,...) file.
Args:
filename (str): path to a DEM, GIS, or image file (e.g. png)
subsample (int, None): if not None, it is the number of points to sub-sample (to smooth the heightmap using
the specified function)
interpolate_fct (str, callable): "The radial basis function, based on the radius, r, given by the norm
(default is Euclidean distance);
'multiquadric': sqrt((r/self.epsilon)**2 + 1)
'inverse': 1.0/sqrt((r/self.epsilon)**2 + 1)
'gaussian': exp(-(r/self.epsilon)**2)
'linear': r
'cubic': r**3
'quintic': r**5
'thin_plate': r**2 * log(r)
If callable, then it must take 2 arguments (self, r). The epsilon parameter will be available as
self.epsilon. Other keyword arguments passed in will be available as well." [1]
min_height (int,float): lower bound; each value in the heightmap will be higher than or equal to this bound
max_height (int,float): upper bound; each value in the heightmap will be lower than or equal to this bound
dtype (np.int, np.float): type of the returned array for the heightmap
Returns:
np.array[H,W]: resulting 2D array of size width `W` and height `H`
Examples:
>>> # generate heightmap from an image or tif file
>>> dem = gdal_heightmap('../pictures/canyon-geo.tif')
>>> dem = gdal_heightmap('../pictures/dem.jpg')
>>> dem = gdal_heightmap('../pictures/heightmap.png')
References:
[1] https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.Rbf.html
"""
# load data (raster)
data = gdal.Open(filename)
band = data.GetRasterBand(1)
heightmap = band.ReadAsArray() # elevation values
if isinstance(subsample, int) and subsample > 0:
height, width = heightmap.shape
idx_x = np.linspace(0, height-1, subsample, dtype=np.int)
idx_y = np.linspace(0, width-1, subsample, dtype=np.int)
idx_x, idx_y = np.meshgrid(idx_x, idx_y)
x, y = np.arange(width), np.arange(height)
x, y = np.meshgrid(x, y)
rbf = Rbf(x[idx_x, idx_y], y[idx_x, idx_y], heightmap[idx_x, idx_y], function=interpolate_fct)
# Nx, Ny = x.shape[0] / subsample, x.shape[1] / subsample
# rbf = Rbf(x[::Nx, ::Ny], y[::Nx, ::Ny], heightmap[::Nx, ::Ny], function=interpolate_fct)
heightmap = rbf(x, y)
# make sure the values of the heightmap are between the bounds (in-place), and is the correct type
if min_height and max_height:
np.clip(heightmap, min_height, max_height, heightmap)
elif min_height:
np.clip(heightmap, min_height, heightmap.max(), heightmap)
elif max_height:
np.clip(heightmap, heightmap.min(), max_height, heightmap)
if dtype:
heightmap.astype(dtype)
return heightmap
pyrobolearn/worlds/utils/heightmaps/gpr.py
+109
View File
@@ -0,0 +1,109 @@
#!/usr/bin/env python
"""Provide the gaussian process regression heightmap generator.
Generate a heightmap using gaussian process regression.
"""
import numpy as np
from sklearn.gaussian_process import GaussianProcessRegressor
from sklearn.gaussian_process.kernels import RBF
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
def gpr_heightmap(init_values, x, y, kernel=None, alpha=1e-10, min_height=0, max_height=255, dtype=np.int):
r"""
Generate a heightmap using gaussian process regression. The advantages of using this method over others to
generate terrains lies in the capacity of adding prior knowledge through the kernel and the given initial values.
For instance, using a RBF kernel means that we want a smooth terrain instead of a bumpy one.
Furthermore, it allows to generate heightmaps which are not necessary square; i.e. they can be rectangular.
Warnings: this is pretty difficult to exploit if the given data is not consistent. See `heigthmap_rbf` for
a better way to generate heightmap.
Args:
init_values (np.array[M,3]): list of `M` 3D points which corresponds to initial values that are used to fit
the gaussian process.
x (np.array[N], np.array[N,O]): If 1d array, it will compute the meshgrid. Otherwise, the resulting 2D array
from the meshgrid is expected. This is used to predict the heightmap at the given points.
y (np.array[0], np.array[N,O]): If 1d array, it will compute the meshgrid. Otherwise, the resulting 2D array
from the meshgrid is expected. This is used to predict the heightmap at the given points.
kernel (None, sklearn.gaussian_process.kernels.Kernel): "The kernel specifying the covariance function of
the GP. If None is passed, the kernel '1.0 * RBF(1.0)' is used as default. Note that the kernel's
hyperparameters are optimized during fitting" [2]
alpha (float, array_like): "Value added to the diagonal of the kernel matrix during fitting. Larger values
correspond to increased noise level in the observations. This can also prevent a potential numerical issue
during fitting, by ensuring that the calculated values form a positive definite matrix. If an array is
passed, it must have the same number of entries as the data used for fitting and is used as
datapoint-dependent noise level. Note that this is equivalent to adding a WhiteKernel with c=alpha.
Allowing to specify the noise level directly as a parameter is mainly for convenience and for consistency
with Ridge." [2]
min_height (int,float): lower bound; each value in the heightmap will be higher than or equal to this bound
max_height (int,float): upper bound; each value in the heightmap will be lower than or equal to this bound
dtype (np.int, np.float): type of the returned array for the heightmap
Returns:
np.array[N,O]: resulting 2D heightmap
Examples:
>>> # generate heightmap using gaussian process regression
>>> x = np.array(range(256))
>>> y = np.array(range(256))
>>> N_init = 20
>>> x_init = np.random.randint(low=x.min(), high=x.max(), size=N_init)
>>> y_init = np.random.randint(low=y.min(), high=y.max(), size=N_init)
>>> z_init = np.random.randint(low=0, high=20, size=N_init)
>>> init_values = np.vstack((x_init, y_init, z_init)).T # shape: Nx3
>>> heightmap = gpr_heightmap(init_values, x, y)
References:
[1] "Gaussian Processes for Machine Learning", Rasmussen and Williams, 2006
[2] Sklearn: https://scikit-learn.org/stable/modules/gaussian_process.html
"""
# check given x and y
if len(x.shape) == 1 and len(y.shape) == 1:
x, y = np.meshgrid(x, y)
if x.shape != y.shape:
raise ValueError("Expecting x and y to have the same shape, which should be the case if it is a meshgrid")
# compute the minimum distance between points
N = len(init_values)
min_dist = np.inf
for i in range(N):
for j in range(i+1, N):
dist = np.linalg.norm(init_values[i, :2] - init_values[j, :2])
if dist < min_dist:
min_dist = dist
print("Min dist: {}".format(min_dist))
# check initial values
if not isinstance(init_values, np.ndarray):
raise TypeError("Expecting init_values to be a numpy array")
if init_values.shape[1] != 3:
raise ValueError("Expecting a numpy array of 3D points for init_values")
# create gaussian process and fit on the given initial values
kernel = RBF(length_scale=np.sqrt(min_dist))
gpr = GaussianProcessRegressor(kernel=kernel, alpha=alpha, normalize_y=True)
gpr.fit(init_values[:, :2], init_values[:, 2])
# predict the heightmap using GPR
X = np.dstack((x, y)).reshape(-1, 2)
heightmap = gpr.predict(X)
heightmap = heightmap.reshape(x.shape)
print("Params: {}".format(gpr.get_params()))
# make sure the values of the heightmap are between the bounds (in-place), and is the correct type
np.clip(heightmap, min_height, max_height, heightmap)
heightmap.astype(dtype)
return heightmap
pyrobolearn/worlds/utils/heightmaps/plot.py
+77
View File
@@ -0,0 +1,77 @@
#!/usr/bin/env python
"""Plot 2D and 3D heightmap.
"""
import numpy as np
import matplotlib.pyplot as plt
from PIL import Image
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
def plot_heightmap(heightmap, title='', block=True, max_height=None):
"""
Plot the given heightmap.
Args:
heightmap (np.array[height, width]): 2D heightmap.
title (str): title of the plot.
block (bool): if we should block when showing the plot.
max_height (None, int, float): max height (z-limit).
"""
fig = plt.figure()
fig.suptitle(title)
# 1st subplot: 2D heightmap
ax = fig.add_subplot(1, 2, 1)
ax.set_title('2D heightmap')
ax.imshow(heightmap, cmap='gray')
# 2nd subplot: associated 3D terrain
ax = fig.add_subplot(1, 2, 2, projection='3d')
ax.set_title('3D terrain')
x = np.linspace(0, 1, heightmap.shape[0])
y = np.linspace(0, 1, heightmap.shape[1])
x, y = np.meshgrid(y, x)
ax.plot_surface(x, y, heightmap)
if max_height is not None:
ax.set_zlim(0, max_height)
plt.show(block=block)
def save_heightmap(heightmap, filename='heightmap.bmp'):
"""
Save the heightmap as an image.
Args:
heightmap (np.array[height, width]): 2D heightmap.
filename (str): filename to save the image.
"""
min_height = heightmap.min()
max_height = heightmap.max()
height, width = heightmap.shape
# create heightmap image
img = Image.new('RGB', (height, width), "black")
pixels = img.load()
dist = (max_height - min_height)
middle_point = min_height + dist / 2.
for i in range(height):
for j in range(width):
if heightmap[i, j] > middle_point:
pixels[i, j] = (0, int(255 * (heightmap[i, j] - middle_point) / (dist / 2.)), 0)
else:
pixels[i, j] = (10, 10, 200)
# save image
img.save(filename)
pyrobolearn/worlds/utils/heightmaps/rbf.py
+81
View File
@@ -0,0 +1,81 @@
#!/usr/bin/env python
"""Provide the radial-basis function heightmap generator.
Generate a heightmap by interpolating the given initial points using RBF functions.
"""
import numpy as np
from scipy.interpolate import Rbf
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
def rbf_heightmap(init_values, x, y, function='multiquadric', min_height=0, max_height=255, dtype=np.int):
r"""
Generate heightmap by interpolating the given initial points using RBF functions.
Advantages: fast and easy to use, and the results are pretty good. Heightmaps can also be rectangular.
Args:
init_values (np.array[M,3]): list of `M` 3D points which corresponds to initial values that are used to fit
the gaussian process.
x (np.array[N], np.array[N,O]): If 1d array, it will compute the meshgrid. Otherwise, the resulting 2D array
from the meshgrid is expected. This is used to predict the heightmap at the given points.
y (np.array[0], np.array[N,O]): If 1d array, it will compute the meshgrid. Otherwise, the resulting 2D array
from the meshgrid is expected. This is used to predict the heightmap at the given points.
function (str, callable): "The radial basis function, based on the radius, r, given by the norm
(default is Euclidean distance);
'multiquadric': sqrt((r/self.epsilon)**2 + 1)
'inverse': 1.0/sqrt((r/self.epsilon)**2 + 1)
'gaussian': exp(-(r/self.epsilon)**2)
'linear': r
'cubic': r**3
'quintic': r**5
'thin_plate': r**2 * log(r)
If callable, then it must take 2 arguments (self, r). The epsilon parameter will be available as
self.epsilon. Other keyword arguments passed in will be available as well." [1]
min_height (int,float): lower bound; each value in the heightmap will be higher than or equal to this bound
max_height (int,float): upper bound; each value in the heightmap will be lower than or equal to this bound
dtype (np.int, np.float): type of the returned array for the heightmap
Returns:
np.array[N,O]: resulting 2D heightmap
Examples:
>>> # generate heightmap using RBF interpolations
>>> x = np.array(range(256))
>>> y = np.array(range(256)) # range(128)
>>> N_init = 20 # number of bumps
>>> x_init = np.random.randint(low=x.min(), high=x.max(), size=N_init)
>>> y_init = np.random.randint(low=y.min(), high=y.max(), size=N_init)
>>> z_init = np.random.randint(low=0, high=20, size=N_init)
>>> init_values = np.vstack((x_init, y_init, z_init)).T # shape: Nx3
>>> heightmap = rbf_heightmap(init_values, x, y, function='gaussian') # 'linear', 'multiquadric'
References:
[1] https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.Rbf.html
"""
# check given x and y
if len(x.shape) == 1 and len(y.shape) == 1:
x, y = np.meshgrid(x, y)
if x.shape != y.shape:
raise ValueError("Expecting x and y to have the same shape, which should be the case if it is a meshgrid")
origin_shape = x.shape
rbf = Rbf(init_values[:, 0], init_values[:, 1], init_values[:, 2], function=function)
heightmap = rbf(x.reshape(-1), y.reshape(-1))
heightmap = heightmap.reshape(origin_shape)
# make sure the values of the heightmap are between the bounds (in-place), and is the correct type
np.clip(heightmap, min_height, max_height, heightmap)
heightmap.astype(dtype)
return heightmap
pyrobolearn/worlds/utils/obj_generator.py
+334
View File
@@ -0,0 +1,334 @@
#!/usr/bin/env python
"""OBJ generator.
Create an OBJ file from a heightmap (2D np.array).
References:
[1] https://github.com/deltabrot/random-terrain-generator
"""
import numpy as np
import time
__author__ = ["Jamie Scollay", "Brian Delhaisse"]
# the code was originally written by Jamie Scollay
# it was then reviewed by Brian Delhaisse, notably with respect to the original code:
# - it has been cleaned; removed all the ";"
# - it has been optimized:
# - it now uses numpy instead of math and lists, it uses `list.append` instead of adding strings, and then `join`
# - it has been simplified.
# - comments have been added and a better documentation is provided
__credits__ = ["Jamie Scollay"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
def unit_vector(v):
"""Normalize the given vector.
Args:
v (np.array[3]): vector to normalize.
Returns:
np.array[3]: unit vector
"""
magnitude = np.linalg.norm(v)
if magnitude == 0:
return v
return v / magnitude
def unit_normal(v1, v2, v3):
"""Compute the unit normal between two vectors: (v2-v1) and (v3 - v1).
Args:
v1 (np.array[3]): 3d common point between the two vectors.
v2 (np.array[3]): 3d point for 1st vector.
v3 (np.array[3]): 3d point for 2nd vector.
Returns:
np.array[3]: unit vector
"""
return unit_vector(np.cross(v2 - v1, v3 - v1))
def create_hexagonal_terrain(heightmap, scale=1., tile=True, min_height=0., smooth=True, verbose=True, verbose_rate=1):
"""
Create the terrain with hexagonal tiles from the heightmap, and return the vertices, textures, normals and faces
to create an OBJ file.
Args:
heightmap (np.array[height, width]): 2D square heightmap
scale (float): scaling factor.
tile (bool): if True, it will create a tile texture.
min_height (float): minimum height.
smooth (bool): if the normals should be smooth.
verbose (bool): if True, it will output information about the creation of the terrain.
verbose_rate (int): if :attr:`verbose` is True, it will output
Returns:
list: vertices (for OBJ)
list: textures (for OBJ)
list: (smooth) normals (for OBJ)
list: faces (for OBJ)
References:
[1] Wavefront .obj file (Wikipedia): https://en.wikipedia.org/wiki/Wavefront_.obj_file
"""
# The heightmap contains the height for each point on the map. If you have 3 points, then you have two
# tiles/segments. Number of segments = number of square tiles in rows or columns.
height, width = heightmap.shape
num_y_tiles, num_x_tiles = height - 1, width - 1
scale = float(scale)
vertices = []
vertices_obj = []
textures_obj = []
normals_obj = []
faces_obj = []
prevent_output = False
if tile:
textures_obj = [[0, 0], [0, 1], [1, 0], [1, 1]]
for i in range(num_y_tiles):
if i == num_y_tiles - 1:
tmp_vertices_obj = []
for j in range(num_x_tiles):
if not smooth:
tmp = 2 * (i * num_x_tiles + j)
# add 2 faces each one composed of 3 vertices/textures/normals (with the format: vertex/texture/normal)
faces_obj.append(str(i * width + j + 1) + '/' + str(1) + '/' + str(tmp + 1) + ' ' +
str(i * width + j + 2) + '/' + str(2) + '/' + str(tmp + 1) + ' ' +
str((i + 1) * width + j + 1) + '/' + str(3) + '/' + str(tmp + 1))
faces_obj.append(str((i + 1) * width + j + 1) + '/' + str(3) + '/' + str(tmp + 2) + ' ' +
str(i * width + j + 2) + '/' + str(2) + '/' + str(tmp + 2) + ' ' +
str((i + 1) * width + j + 2) + '/' + str(4) + '/' + str(tmp + 2))
else:
# add 2 faces each one composed of 3 vertices/textures/normals (with the format: vertex/texture/normal)
faces_obj.append(str(i * width + j + 1) + '/' + str(1) + '/' + str(i * width + j + 1) + ' ' +
str(i * width + j + 2) + '/' + str(2) + '/' + str(i * width + j + 2) + ' ' +
str((i + 1) * width + j + 1) + '/' + str(3) + '/' + str((i + 1) * width + j + 1))
faces_obj.append(str((i + 1) * width + j + 1) + '/' + str(3) + '/' + str((i + 1)*width + j + 1) + ' ' +
str(i * width + j + 2) + '/' + str(2) + '/' + str(i * width + j + 2) + ' ' +
str((i + 1) * width + j + 2) + '/' + str(4) + '/' + str((i + 1) * width + j + 2))
# T1
half_scale = scale / 2.
scale_tile = scale / num_x_tiles
# add vertex [x,y,z] for the obj file
vertices_obj.append([-half_scale + i * scale_tile,
heightmap[i, j],
-half_scale + j * scale_tile])
if j == num_x_tiles - 1:
vertices_obj.append([-half_scale + i * scale_tile,
heightmap[i, j+1],
-half_scale + (j+1) * scale_tile])
if i == num_y_tiles - 1:
tmp_vertices_obj.append([-half_scale + (i+1) * scale_tile,
heightmap[i+1, j],
-half_scale + j * scale_tile])
if j == num_x_tiles - 1:
tmp_vertices_obj.append([-half_scale + (i+1) * scale_tile,
heightmap[i+1, j+1],
-half_scale + (j+1) * scale_tile])
# T1: add 3 vertices [x,y,z] (that are used to compute the vertex normal)
vertices.append(np.array([-half_scale + i * scale_tile,
heightmap[i, j],
-half_scale + j * scale_tile]))
vertices.append(np.array([-half_scale + i * scale_tile,
heightmap[i, j + 1],
-half_scale + (j+1) * scale_tile]))
vertices.append(np.array([-half_scale + (i+1) * scale_tile,
heightmap[i + 1, j],
-half_scale + j * scale_tile]))
# else:
# textures.append([i/segment, j/segment])
# textures.append([i/segment, (j+1)/segment])
# textures.append([(i+1)/segment, j/segment])
# compute vertex normal [x,y,z] based on last 3 vertices
normal = unit_normal(vertices[-3], vertices[-2], vertices[-1])
normals_obj.append(normal)
# T2: add 3 vertices [x,y,z] (that are used to compute the vertex normal)
vertices.append(np.array([-half_scale + (i+1) * scale_tile,
heightmap[i+1, j],
-half_scale + j * scale_tile]))
vertices.append(np.array([-half_scale + i * scale_tile,
heightmap[i, j+1],
-half_scale + (j+1) * scale_tile]))
vertices.append(np.array([-half_scale + (i+1) * scale_tile,
heightmap[i+1, j+1],
-half_scale + (j+1) * scale_tile]))
# else:
# textures.append([(i+1)/segment, j/segment])
# textures.append([i/segment, (j+1)/segment])
# textures.append([(i+1)/segment, (j+1)/segment])
# compute vertex normal [x,y,z] based on last 3 vertices
normal = unit_normal(vertices[-3], vertices[-2], vertices[-1])
normals_obj.append(normal)
# print information if specified
if verbose:
if (time.time() % verbose_rate) < 0.05 and not prevent_output:
print("{}/{} tiles".format(i * num_x_tiles + j, num_x_tiles * num_y_tiles))
prevent_output = True
elif time.time() % verbose_rate > 0.05:
prevent_output = False
vertices_obj += tmp_vertices_obj
# if we don't have to smooth the normals
if not smooth:
return [vertices_obj, textures_obj, normals_obj, faces_obj]
# smooth the normals using 6 normals
smooth_normals = []
for i in range(num_y_tiles):
tmp_smooth_normals = []
for j in range(num_x_tiles):
if j > 0:
norm0 = normals_obj[(i * num_x_tiles + (j - 1) * 2)]
norm1 = normals_obj[(i * num_x_tiles + (j - 1) * 2) + 1]
else:
norm0 = np.zeros(3)
norm1 = np.zeros(3)
norm2 = normals_obj[((i * num_x_tiles + j) * 2)]
if i > 0 and j > 0:
norm3 = normals_obj[(((i - 1) * num_x_tiles + (j - 1)) * 2) + 1]
else:
norm3 = np.zeros(3)
if i > 0:
norm4 = normals_obj[(((i - 1) * num_x_tiles + j) * 2)]
norm5 = normals_obj[(((i - 1) * num_x_tiles + j) * 2) + 1]
else:
norm4 = np.zeros(3)
norm5 = np.zeros(3)
smooth_normals.append(unit_vector(norm0 + norm1 + norm2 + norm3 + norm4 + norm5))
if j == num_x_tiles - 1:
norm0 = normals_obj[((i * num_x_tiles + (j - 1)) * 2)]
norm1 = normals_obj[((i * num_x_tiles + (j - 1)) * 2) + 1]
if i > 0:
norm2 = normals_obj[(((i - 1) * num_x_tiles + (j - 1)) * 2) + 1]
else:
norm2 = np.zeros(3)
smooth_normals.append(unit_vector(norm0 + norm1 + norm2))
if i == num_y_tiles - 1:
if j > 0:
norm0 = normals_obj[(((i - 1) * num_x_tiles + (j - 1)) * 2) + 1]
else:
norm0 = np.zeros(3)
norm1 = normals_obj[(((i - 1) * num_x_tiles + j) * 2)]
norm2 = normals_obj[(((i - 1) * num_x_tiles + j) * 2) + 1]
tmp_smooth_normals.append(unit_vector(norm0 + norm1 + norm2))
if j == num_x_tiles - 1:
norm0 = normals_obj[(((i - 1) * num_x_tiles + (j - 1)) * 2) + 1]
tmp_smooth_normals.append(norm0)
smooth_normals += tmp_smooth_normals
return [vertices_obj, textures_obj, smooth_normals, faces_obj]
def create_obj(vertices, textures, normals, faces, filename=None):
"""
Create content of the OBJ file given the vertices, textures, normals, and faces.
Args:
vertices (list): list of vertices (for each corner of a triangular mesh)
textures (list): list of textures
normals (list): list of normals
faces (list): list of faces
filename (None, str): if a string is provided, it will save the OBJ file in the given file path.
Returns:
str: content of the OBJ file.
References:
[1] Wavefront .obj file (Wikipedia): https://en.wikipedia.org/wiki/Wavefront_.obj_file
"""
# create obj list
obj = []
# add vertices
for v in vertices:
obj.append("v " + str(v[0]) + " " + str(v[1]) + " " + str(v[2]))
# add textures
for t in textures:
obj.append("vt " + str(t[0]) + " " + str(t[1]))
# add normals
for n in normals:
obj.append("vn " + str(n[0]) + " " + str(n[1]) + " " + str(n[2]))
# add faces
for f in faces:
obj.append("f " + f)
# create document
obj = '\n'.join(obj)
# create file if specified
if filename is not None:
with open(filename, "w+") as f:
f.write(obj)
return obj
def create_obj_from_heightmap(heightmap, scale=1., tile=True, min_height=0., smooth=True, verbose=True, verbose_rate=1,
filename=None):
"""
Create an OBJ file from the given 2D heightmap.
Args:
heightmap (np.array[height, width]): 2D square heightmap
scale (float): scaling factor.
tile (bool): if True, it will create a tile texture.
min_height (float): minimum height.
smooth (bool): if the normals should be smooth.
verbose (bool): if True, it will output information about the creation of the terrain.
verbose_rate (int): if :attr:`verbose` is True, it will output
filename (None, str): if a string is provided, it will save the OBJ file in the given file path.
Returns:
str: content of the OBJ file.
References:
[1] Wavefront .obj file (Wikipedia): https://en.wikipedia.org/wiki/Wavefront_.obj_file
"""
# create vertices, textures, normals, and faces
terrain = create_hexagonal_terrain(heightmap, scale, tile, min_height, smooth, verbose, verbose_rate)
# create obj based on above information
obj = create_obj(vertices=terrain[0], textures=terrain[1], normals=terrain[2], faces=terrain[3],
filename=filename)
return obj
pyrobolearn/worlds/utils/pictures/canyon-geo.tif
Executable
BIN
View File
Binary file not shown.
pyrobolearn/worlds/utils/pictures/dem.jpg
Executable
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 16 KiB

pyrobolearn/worlds/utils/pictures/heightmap.png
Executable
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 5.8 KiB

pyrobolearn/worlds/utils/pictures/map.bmp
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 194 KiB

pyrobolearn/worlds/utils/terrain_generator.py
-403
View File
@@ -1,403 +0,0 @@
#!/usr/bin/env python
"""Random terrain generator using the Diamond square algorithm.
It generates a random heightmap / terrain using the diamond square algorithm, and outputs an OBJ file.
The code comes from [1], and has been optimized.
References:
[1] https://github.com/deltabrot/random-terrain-generator
"""
import time
from PIL import Image
import numpy as np
__author__ = ["Jamie Scollay", "Brian Delhaisse"]
# the code was originally written by Jamie Scollay
# it was then reviewed by Brian Delhaisse, notably with respect to the original code:
# - it has been cleaned; removed all the ";"
# - it has been optimized: it now uses numpy instead of math and lists, it uses list.append instead of adding strings
# - it is now better documented.
__credits__ = ["Jamie Scollay"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
def unit_vector(v):
"""Normalize the given vector.
Args:
v (np.float[3]): vector to normalize.
Returns:
np.float[3]: unit vector
"""
magnitude = np.linalg.norm(v)
if magnitude == 0:
return v
return v / magnitude
def unit_normal(v1, v2, v3):
"""Compute the unit normal between two vectors: (v2-v1) and (v3 - v1).
Args:
v1 (np.float[3]): 3d common point between the two vectors.
v2 (np.float[3]): 3d point for 1st vector.
v3 (np.float[3]): 3d point for 2nd vector.
Returns:
np.float[3]: unit vector
"""
return unit_vector(np.cross(v2 - v1, v3 - v1))
def display_loading(count, finish, message):
"""
Display loading message.
Args:
count (int): each row of the surface.
finish (int): surface.
message (str): message to print.
"""
print(message + ": " + str(count) + "/" + str(finish))
def diamond_square_heightmap(n, max_height, jitter, jitter_factor):
"""
Create a 2D square heightmap using the diamond square algorithm [1]
Args:
n (int): used to create a square array of width and height of 2**n + 1. It also specifies the number of
diamond and square steps.
max_height (float): maximum height
jitter (float): magnitude of the noise added to the computed height.
jitter_factor (float): after each step, the jitter is divided by the given factor.
Returns:
np.float[size, size]: 2D square heightmap
References:
[1] Wikipedia: https://en.wikipedia.org/wiki/Diamond-square_algorithm
"""
# compute the width and height size (i.e. size of the 2D square array/heightmap)
size = int(2**n + 1)
# create initial heightmap of width and height size
heightmap = np.zeros((size, size))
# assign a random height at each corner of the heightmap
heightmap[0, 0] = np.random.rand() * max_height
heightmap[0, size-1] = np.random.rand() * max_height
heightmap[size-1, 0] = np.random.rand() * max_height
heightmap[size-1, size-1] = np.random.rand() * max_height
# for each diamond and square step
for i in range(n):
stride = int((size-1) / 2**(i+1))
radius = int((size-1) / 2**i)
for j in range(2**i):
for k in range(2**i):
height = (heightmap[j*radius, k*radius] + heightmap[2*stride + j*radius, k*radius] +
heightmap[j*radius, 2*stride + k*radius] +
heightmap[2*stride + j*radius, 2*stride + k*radius]) / 4. + (np.random.rand()-0.5) * jitter
heightmap[stride + j*radius, stride + k*radius] = height
for j in range(2**(i+1) + 1):
for k in range(2**i + j%2):
cnt = 0
if j == 0:
height1 = 0
else:
height1 = heightmap[(j-1) * stride, stride*((j+1)%2) + k*radius]
cnt += 1
if k == 0 and j%2 == 1:
height4 = 0
else:
height4 = heightmap[j * stride, stride*(((j+1)%2)-1) + k*radius]
cnt += 1
if j == 2**(i+1):
height3 = 0
else:
height3 = heightmap[(j+1) * stride, stride*((j+1)%2) + k*radius]
cnt += 1
if k == (2**i + j%2 - 1) and j%2 == 1:
height2 = 0
else:
height2 = heightmap[j*stride, stride*(((j+1)%2)+1) + k*radius]
cnt += 1
height = float(height1 + height2 + height3 + height4) / cnt + (np.random.rand()-0.5) * jitter
heightmap[j*stride, ((j+1)%2) * stride + k*radius] = height
jitter /= jitter_factor
lowest_point = heightmap.min()
highest_point = heightmap.max()
if n > 1:
for i in range(size):
for j in range(4):
heightmap[j, i] = lowest_point + j * 0.25 * (heightmap[j, i] - lowest_point)
heightmap[size-j-1, i] = lowest_point + j * 0.25 * (heightmap[size-j-1, i] - lowest_point)
heightmap[i, j] = lowest_point + j * 0.25 * (heightmap[i, j] - lowest_point)
heightmap[i, size-j-1] = lowest_point + j * 0.25 * (heightmap[i, size-j-1] - lowest_point)
# create heightmap image
img = Image.new('RGB', (size, size), "black")
pixels = img.load()
dist = (highest_point - lowest_point)
middle_point = lowest_point + dist/2.
for i in range(size):
for j in range(size):
if heightmap[i, j] > middle_point:
pixels[i, j] = (0, int(255 * (heightmap[i, j] - middle_point) / (dist/2.)), 0)
else:
pixels[i, j] = (10, 10, 200)
# save image
img.save("map.bmp")
return heightmap
def create_hexagonal_terrain(segment, scale, tile, min_height, heightmap, smooth=True, verbose=True, verbose_rate=1):
"""
Create the terrain with hexagonal tiles.
Args:
segment (int): number of segments; number of square tiles in rows or columns.
scale (float): scaling factor.
tile (bool): if True, it will create a tile texture.
min_height (float): minimum height.
heightmap (np.float[size, size]): 2D square heightmap
smooth (bool): if the normals should be smooth.
verbose (bool): if True, it will output information about the creation of the terrain.
verbose_rate (int): if :attr:`verbose` is True, it will output
Returns:
list: vertices (for OBJ)
list: textures (for OBJ)
list: (smooth) normals (for OBJ)
list: faces (for OBJ)
"""
scale = float(scale)
vertices = []
vertices_obj = []
textures_obj = []
normals_obj = []
facesOBJ = []
prevent_output = False
if tile:
textures_obj = [[0, 0], [0, 1], [1, 0], [1, 1]]
for i in range(segment):
if i == segment-1:
tmp_vertices_obj = []
for j in range(segment):
if not smooth:
tmp = 2 * (i*segment + j)
facesOBJ.append(str(i*(segment+1) + j + 1) + '/' + str(1) + '/' + str(tmp + 1) + ' ' +
str(i*(segment+1) + j + 2) + '/' + str(2) + '/' + str(tmp + 1) + ' ' +
str((i+1)*(segment+1) + j + 1) + '/' + str(3) + '/' + str(tmp + 1))
facesOBJ.append(str((i+1)*(segment+1) + j + 1) + '/' + str(3) + '/' + str(tmp + 2) + ' ' +
str(i*(segment+1) + j + 2) + '/' + str(2) + '/' + str(tmp + 2) + ' ' +
str((i+1)*(segment+1) + j + 2) + '/' + str(4) + '/' + str(tmp + 2))
else:
facesOBJ.append(str(i*(segment+1) + j + 1) + '/' + str(1) + '/' + str(i*(segment+1) + j + 1) + ' ' +
str(i*(segment+1) + j + 2) + '/' + str(2) + '/' + str(i*(segment+1) + j + 2) + ' ' +
str((i+1)*(segment+1) + j + 1) + '/' + str(3) + '/' + str((i+1)*(segment+1) + j + 1))
facesOBJ.append(str((i+1)*(segment+1) + j + 1) + '/' + str(3) + '/' + str((i+1)*(segment+1) + j + 1) +
' ' + str(i*(segment+1) + j + 2) + '/' + str(2) + '/' + str(i*(segment+1) + j + 2) +
' ' + str((i+1)*(segment+1) + j + 2) + '/' + str(4) + '/' +
str((i+1)*(segment+1) + j + 2))
# T1
half_scale = scale / 2.
scale_seg = scale / segment
vertices_obj.append([-half_scale + i*scale_seg, heightmap[i, j], -half_scale + j*scale_seg])
if j == segment-1:
vertices_obj.append([-half_scale + i*scale_seg, heightmap[i, j+1], -half_scale + (j+1)*scale_seg])
if i == segment-1:
tmp_vertices_obj.append([-half_scale + (i+1)*scale_seg, heightmap[i+1, j], -half_scale + j*scale_seg])
if j == segment-1:
tmp_vertices_obj.append([-half_scale + (i+1)*scale_seg, heightmap[i+1, j+1],
-half_scale + (j+1)*scale_seg])
vertices.append(np.array([-half_scale + i*scale_seg,
heightmap[i, j],
-half_scale + j*scale_seg]))
vertices.append(np.array([-half_scale + i*scale_seg,
heightmap[i, j+1],
-half_scale + (j+1)*scale_seg]))
vertices.append(np.array([-half_scale + (i+1)*scale_seg,
heightmap[i+1, j],
-half_scale + j*scale_seg]))
# else:
# textures.append([i/segment, j/segment])
# textures.append([i/segment, (j+1)/segment])
# textures.append([(i+1)/segment, j/segment])
num_vertices = len(vertices)
normal = unit_normal(vertices[num_vertices-3], vertices[num_vertices-2], vertices[num_vertices-1])
normals_obj.append(normal)
# T2
vertices.append(np.array([-half_scale + (i+1)*scale_seg, heightmap[i+1, j], -half_scale + j*scale_seg]))
vertices.append(np.array([-half_scale + i*scale_seg, heightmap[i, j+1], -half_scale + (j+1)*scale_seg]))
vertices.append(np.array([-half_scale + (i+1)*scale_seg, heightmap[i+1, j+1], -half_scale + (j+1)*scale_seg]))
# else:
# textures.append([(i+1)/segment, j/segment])
# textures.append([i/segment, (j+1)/segment])
# textures.append([(i+1)/segment, (j+1)/segment])
num_vertices = len(vertices)
normal = unit_normal(vertices[num_vertices-3], vertices[num_vertices-2], vertices[num_vertices-1])
normals_obj.append(normal)
if verbose:
if (time.time() % verbose_rate) < 0.05 and not prevent_output:
display_loading(i*segment + j, segment*segment, "TER | Segm")
prevent_output = True
elif time.time() % verbose_rate > 0.05:
prevent_output = False
# smooth the normals
smooth_normals = []
for i in range(segment):
tmp_smooth_normals = []
for j in range(segment):
if j > 0:
norm0 = normals_obj[(i*segment + (j-1)*2)]
norm1 = normals_obj[(i*segment + (j-1)*2) + 1]
else:
norm0 = np.zeros(3)
norm1 = np.zeros(3)
norm2 = normals_obj[((i*segment + j)*2)]
if i > 0 and j > 0:
norm3 = normals_obj[(((i-1)*segment + (j-1))*2) + 1]
else:
norm3 = np.zeros(3)
if i > 0:
norm4 = normals_obj[(((i-1)*segment + j)*2)]
norm5 = normals_obj[(((i-1)*segment + j)*2) + 1]
else:
norm4 = np.zeros(3)
norm5 = np.zeros(3)
smooth_normals.append(unit_vector(norm0 + norm1 + norm2 + norm3 + norm4 + norm5))
if j == segment-1:
norm0 = normals_obj[((i*segment + (j-1))*2)]
norm1 = normals_obj[((i*segment + (j-1))*2) + 1]
if i > 0:
norm2 = normals_obj[(((i-1)*segment + (j-1))*2) + 1]
else:
norm2 = np.zeros(3)
smooth_normals.append(unit_vector(norm0 + norm1 + norm2))
if i == segment-1:
if j > 0:
norm0 = normals_obj[(((i-1)*segment + (j-1))*2) + 1]
else:
norm0 = np.zeros(3)
norm1 = normals_obj[(((i-1)*segment + j)*2)]
norm2 = normals_obj[(((i-1)*segment + j)*2) + 1]
tmp_smooth_normals.append(unit_vector(norm0 + norm1 + norm2))
if j == segment-1:
norm0 = normals_obj[(((i-1)*segment + (j-1))*2) + 1]
tmp_smooth_normals.append(norm0)
smooth_normals += tmp_smooth_normals
vertices_obj += tmp_vertices_obj
if smooth:
return [vertices_obj, textures_obj, smooth_normals, facesOBJ]
return [vertices_obj, textures_obj, normals_obj, facesOBJ]
def create_obj(vertices, textures, normals, faces, filename=None):
"""
Create content of the OBJ file.
Args:
vertices (list): list of vertices (for each corner of a triangular mesh)
textures (list): list of textures
normals (list): list of normals
faces (list): list of faces
filename (None, str): if a string is provided, it will save the OBJ file in the given file path.
Returns:
str: content of the OBJ file.
"""
# create obj list
obj = []
for v in vertices:
obj.append("v " + str(v[0]) + " " + str(v[1]) + " " + str(v[2]))
for t in textures:
obj.append("vt " + str(t[0]) + " " + str(t[1]))
for n in normals:
obj.append("vn " + str(n[0]) + " " + str(n[1]) + " " + str(n[2]))
for f in faces:
obj.append("f " + f)
# create document
obj = '\n'.join(obj)
# create file if specified
if filename is not None:
with open(filename, "w+") as f:
f.write(obj)
return obj
segments = 8
scale = 600.
tile = True
max_height = 75.
min_height = 0.
verbose = True
verbose_rate = 1
jitter = 40.
jitter_factor = 1.5
smooth = True
# create heightmap, generate terrain from it, and obj mesh
print("Generating terrain")
start = time.time()
# create heightmap
heightmap = diamond_square_heightmap(segments, max_height, jitter, jitter_factor)
# create vertices, textures, normals, and faces
terrain = create_hexagonal_terrain(2 ** segments, scale, tile, min_height, heightmap, smooth, verbose,
verbose_rate)
# create obj based on above information
obj = create_obj(vertices=terrain[0], textures=terrain[1], normals=terrain[2], faces=terrain[3],
filename='terrain.obj')
end = time.time()
print("Terrain generated in {:.2f} seconds.".format(end - start))
pyrobolearn/worlds/utils/terrains/terrain_heightmap.obj
+82181
View File
File diff suppressed because it is too large Load Diff
pyrobolearn/worlds/utils/terrains/terrain_map.obj
+263174
View File
File diff suppressed because it is too large Load Diff
pyrobolearn/worlds/utils/terrains/terrain_smooth_heightmap.obj
+66054
View File
File diff suppressed because it is too large Load Diff
pyrobolearn/worlds/world.py
+169 -120
View File
@@ -16,11 +16,14 @@ import time
from pyrobolearn.simulators import Simulator
from pyrobolearn.worlds.world_camera import WorldCamera
# from pyrobolearn.utils.heightmap_generator import * # TODO: problem with gdal installation
from pyrobolearn.utils import has_method, has_variable
from pyrobolearn.robots import Body, Robot, robot_names_to_classes
# TODO: to install the `gdal` library, run the script `pyrobolearn/scripts/install_gdal.sh`, by default do not
# import it
from pyrobolearn.worlds.utils.heightmaps.diamond_square import diamond_square_heightmap, diamond_square_heightmap_2
from pyrobolearn.worlds.utils.heightmaps.rbf import rbf_heightmap
from pyrobolearn.worlds.utils.heightmaps.equation import equation_heightmap
from pyrobolearn.worlds.utils.obj_generator import create_obj_from_heightmap
__author__ = "Brian Delhaisse"
@@ -369,6 +372,10 @@ class World(object):
Robot: instance of the Robot class
"""
# TODO check the height of the terrain where we wish to load the robot
def check_height(position):
"""check that the position of the robot is in accordance with the height of the terrain for that
position."""
return position
if isinstance(robot, Robot): # the robot is already loaded, then add it to the list
pass
@@ -381,10 +388,13 @@ class World(object):
robot = robot_class(self.sim, position=position, orientation=orientation, fixed_base=fixed_base,
*args, **kwargs)
else: # robot is the path to the urdf
elif robot[-4:] == 'urdf': # robot is the path to the urdf
robot = Robot(self.sim, urdf=robot, position=position, orientation=orientation, fixed_base=fixed_base,
*args, **kwargs)
else:
raise ValueError("The given string does not correspond to any robots or urdfs: {}".format(robot))
elif inspect.isclass(robot): # robot class
robot = robot(self.sim, position=position, orientation=orientation, fixed_base=fixed_base, *args, **kwargs)
@@ -542,9 +552,6 @@ class World(object):
object_id (int): object id
position (float[3]): new position of the object. If None, it will keep the old position.
orientation (float[4]): new orientation of the object. If None, it will keep the old orientation.
Returns:
None
"""
if position is None:
position = self.sim.get_base_pose(object_id)[0]
@@ -568,9 +575,6 @@ class World(object):
of the object (or the link if specified)
frame (int): allows to specify the coordinate system of force/position. sim.LINK_FRAME (=1) for local
link frame, and sim.WORLD_FRAME (=2) for world frame. By default, it is the world frame.
Returns:
None
"""
if position is None:
if link_id != -1:
@@ -599,9 +603,6 @@ class World(object):
object_id (int): object id
color (float[4]): RGBA color
link_id (int): link id
Returns:
None
"""
self.sim.change_visual_shape(object_id, link_id, rgba_color=color)
@@ -672,9 +673,6 @@ class World(object):
Args:
object_id (int): object id
Returns:
None
"""
color = self.get_object_color(object_id)
color[-1] = 0.
@@ -686,9 +684,6 @@ class World(object):
Args:
object_id (int): object id
Returns:
None
"""
color = self.get_object_color(object_id)
color[-1] = 1.
@@ -739,9 +734,6 @@ class World(object):
Args:
object_id (int): object id
scale (float[3]): scaling factors in each direction
Returns:
None
"""
# TODO: currently not possible in PyBullet
pass
@@ -846,18 +838,22 @@ class World(object):
self.floor_id = self.sim.load_urdf('plane.urdf', use_fixed_base=True, scale=scaling)
return self.floor_id
def load_terrain(self, heightmap, position=(0., 0., 0.), scaling=1., replace_floor=True):
def load_terrain(self, heightmap, position=(0., 0., 0.), orientation=(.707, 0, 0, .707), scaling=1.,
replace_floor=True, remove_obj=False, texture=None):
"""
Load the given terrain/heightmap.
Load the given terrain/heightmap into the world.
Args:
heightmap (str, np.array[heigth,width]): path to the urdf, sdf, xml, or obj file of the terrain. It can
heightmap (str, np.array[heigth, width]): path to the urdf, sdf, xml, or obj file of the terrain. It can
also be the path to a heightmap in tif, jpg, or png format. Alternatively, it can represents
the heightmap as a 2D numpy array where the values represent the height in meters.
position (float[3]): position of the terrain. By default, origin of the world.
scaling (float): scaling factor of the terrain
orientation (tuple of 4 float): orientation of the terrain (expressed as quaternion [x,y,z,w]).
scaling (float, tuple of 3 float): scaling factor of the terrain.
replace_floor (bool): if True, it will replace the existing floor. Be careful, that it can cause
problems with collision.
remove_obj (bool): if True, it will remove the obj file.
texture (str, None): texture to apply.
Returns:
int: unique id of the terrain.
@@ -867,111 +863,101 @@ class World(object):
- `openmesh`: https://www.openmesh.org/media/Documentations/OpenMesh-6.2-Documentation/a00036.html
- `bpy`: Blender python API - https://docs.blender.org/api/current/
"""
if self.floor_id > -1: # there is already a floor defined
# if there is already a floor, remove it if specified
if self.floor_id > -1:
if replace_floor:
self.sim.remove_body(self.floor_id)
if heightmap[-4:] == 'obj': # obj (mesh)
self.floor_id = self.load_mesh(heightmap, position, mass=0., scale=[scaling] * 3, flags=1,
object_type='terrain')
elif heightmap[-4:] == '.sdf': # SDF
self.floor_id = self.load_sdf(filename=heightmap, scaling=scaling)
elif heightmap[-4:] == '.xml': # MJCF
self.floor_id = self.load_mjcf(filename=heightmap, scaling=scaling)
elif heightmap[-5:] == '.urdf': # URDF
self.floor_id = self.sim.load_urdf(heightmap, position, use_fixed_base=True, scale=scaling)
else: # heightmap (.tif, .jpg, .png, etc)
def create_mesh(heightmap):
# create 3D mesh
# create3DMesh(heightmap, filename=, subsample=, interpolate_fct=)
pass
# if heightmap is a 2D array
if isinstance(heightmap, np.ndarray):
self.generate_terrain(heightmap, filename='heightmap.obj')
heightmap = 'heightmap.obj'
# create process to create the 3D mesh
process = multiprocessing.Process(target=create_mesh, args=(heightmap,))
process.start()
process.join()
filename = heightmap
# if heightmap is a string, it is the path to the 3d terrain or image
if isinstance(filename, str):
if filename[-3:] == 'obj': # obj (mesh)
if not isinstance(scaling, (list, tuple)):
scaling = [scaling] * 3
self.floor_id = self.load_mesh(filename, position, orientation, mass=0., scale=scaling, flags=1,
object_type='terrain')
elif filename[-3:] == 'sdf': # SDF
self.floor_id = self.load_sdf(filename=filename, scaling=scaling)
elif filename[-3:] == 'xml': # MJCF
self.floor_id = self.load_mjcf(filename=filename, scaling=scaling)
elif filename[-4:] == 'urdf': # URDF
self.floor_id = self.sim.load_urdf(filename, position, use_fixed_base=True, scale=scaling)
else: # heightmap (.tif, .jpg, .png, etc)
# if extension is jpg or png
if filename[-3:] == 'png' or filename[-3:] == 'jpg':
heightmap = cv2.imread(filename, cv2.IMREAD_GRAYSCALE)
else: # use gdal to open the heightmap
heightmap = self.generate_heightmap(algo=6, filename=filename)
self.generate_terrain(heightmap, filename=filename)
# load the obj
if not isinstance(scaling, (list, tuple)):
scaling = [scaling] * 3
self.floor_id = self.load_mesh(filename, position, orientation, mass=0., scale=scaling, flags=1,
object_type='terrain')
else:
raise TypeError("Expecting the given 'heightmap' to be a string or a numpy array, instead got: "
"{}".format(type(heightmap)))
if filename[-3:] == 'obj' and remove_obj:
# remove mesh from memory
os.remove(filename + '.obj') # remove mesh from memory
# os.remove(filename + '.mtl')
# apply the given texture if provided
if isinstance(texture, str):
texture = self.sim.load_texture(texture)
self.sim.change_visual_shape(heightmap, -1, texture_id=texture)
# apply the given texture if provided
if isinstance(texture, str):
texture = self.sim.load_texture(texture)
self.sim.change_visual_shape(object_id=self.floor_id, link_id=-1, texture_id=texture)
# return the floor id
return self.floor_id
def load_heightmap(self, heightmap, texture=None, position=(0., 0., 0.), scale=1.):
def load_heightmap(self, filename):
"""
Load a heightmap for the terrain.
Load a heightmap from an image.
Args:
heightmap (str, np.ndarray[M,M]): if string, filename containing the heightmap in the png, jpg, obj format
if a 2D numpy arrays, the values represent the height in meters.
texture: texture to apply
position (float[3]): position of the terrain
scale (float): scaling factor
filename (str): filename containing the heightmap in the png, jpg, tif, bmp format
Returns:
int: unique id of the floor
np.array[H,W]: heightmap (height, width)
Modules to create mesh files (.obj):
- `mayavi`: https://docs.enthought.com/mayavi/mayavi/
- `openmesh`: https://www.openmesh.org/media/Documentations/OpenMesh-6.2-Documentation/a00036.html
- `bpy`: Blender python API - https://docs.blender.org/api/current/
"""
if not isinstance(filename, str):
raise TypeError("Expecting the given 'filename' to be a string (i.e. the path to the heightmap image), "
"instead got: {}".format(type(filename)))
extension, filename = None, 'generated_file'
# load heightmap
if filename[-3:] == 'png' or filename[-3:] == 'jpg':
heightmap = cv2.imread(filename, cv2.IMREAD_GRAYSCALE)
else: # use gdal to open the heightmap
heightmap = self.generate_heightmap(algo=6, filename=filename)
# if string, get the extension and name of the file
if isinstance(heightmap, str):
extension = heightmap.split('.')[-1]
filename = heightmap[:-4]
# if picture (png/jpg), load 2D array (grayscale values)
if extension == 'png' or extension == 'jpg':
heightmap = cv2.imread(filename, cv2.IMREAD_GRAYSCALE)
# if 2D numpy array, create the mesh (in the .obj format)
if isinstance(heightmap, np.ndarray):
heightmap = heightmap.astype(np.float)
mlab.surf(heightmap)
mlab.savefig(filename + '.obj')
mlab.close()
# TODO: set the map of the world
elif extension != 'obj':
raise ValueError("Expecting heightmap in a png/jpg/obj format")
# load the mesh of the terrain
heightmap = self.load_terrain(filename, position=position, scaling=scale)
# change the dynamic properties of the terrain based on the given type (grass, mud, bumpy
# WARNING: only 1 type can be specified. Currently, it is not possible to have different dynamic properties
# for different parts of the terrain. Loading multiple terrains is currently not supported (there is only
# 1 unique id for the floor).
# apply the given texture if provided
if isinstance(texture, str):
texture = self.sim.load_texture(texture)
self.sim.change_visual_shape(heightmap, -1, texture_id=texture)
# remove mesh from memory
os.remove(filename + '.obj') # remove mesh from memory
os.remove(filename + '.mtl')
# replace the floor if there is already one present
if self.floor_id > -1:
self.sim.remove_body(self.floor_id)
self.floor_id = heightmap
return self.floor_id
return heightmap
# aliases
loadDEM = load_heightmap
def generate_heightmap(self, filename=None, algo=None):
@staticmethod
def generate_heightmap(algo=2, filename=None, width=256, height=256, n=8, min_height=0, max_height=255, noise=0,
noise_factor=1, init_values=None, x=None, y=None, z=None, function='multiquadric',
dtype=np.int):
"""
Generate a heightmap (png) using the specified algorithm. We provide 4 algorithms to generate this last one:
1. by generating it randomly (not advised)
@@ -987,23 +973,85 @@ class World(object):
approaches.
Args:
filename (None, str): if not None, it will save the heightmap in the format specified by the filename.
The format is inferred from the filename. Supported ones include '.png', '.jpg', and '.obj'.
algo (int): specifies which algorithm to use to generate the heightmap.
1. randomly
2. diamond-square algorithm
3. diamond-square algorithm (version 2)
4. RBF interpolation
5. 3d equation
6. geospatial
filename (str, None): if algo=6, path to a DEM, GIS, or image file (e.g. png) to open.
n (int): used to create a square array of width and height of 2**n + 1. It also specifies the number of
diamond and square steps.
min_height (int,float): lower bound; each value in the heightmap will be higher than or equal to this bound
max_height (int,float): upper bound; each value in the heightmap will be lower than or equal to this bound
noise (float): magnitude of the noise added to the computed height.
noise_factor (float): after each step, the jitter is divided by the given factor.
init_values (np.array[M,3]): list of `M` 3D points which corresponds to initial values that are used to fit
the gaussian process.
x (np.array[N], np.array[N,O]): If 1d array, it will compute the meshgrid. Otherwise, the resulting 2D
array from the meshgrid is expected. This is used to predict the heightmap at the given points.
y (np.array[0], np.array[N,O]): If 1d array, it will compute the meshgrid. Otherwise, the resulting 2D
array from the meshgrid is expected. This is used to predict the heightmap at the given points.
z (callable): it must be a function that accepts two arguments `x` and `y` which will be the arrays from
the meshgrid.
function (str, callable): "The radial basis function, based on the radius, r, given by the norm
(default is Euclidean distance);
'multiquadric': sqrt((r/self.epsilon)**2 + 1)
'inverse': 1.0/sqrt((r/self.epsilon)**2 + 1)
'gaussian': exp(-(r/self.epsilon)**2)
'linear': r
'cubic': r**3
'quintic': r**5
'thin_plate': r**2 * log(r)
If callable, then it must take 2 arguments (self, r). The epsilon parameter will be available as
self.epsilon. Other keyword arguments passed in will be available as well."
dtype (np.int, np.float): type of the returned array for the heightmap
Returns:
np.array[W,H]: heightmap (with, height)
np.array[H,W]: heightmap (height, width)
"""
pass
if algo == 1: # random
heightmap = np.random.randint(min_height, max_height)
elif algo == 2: # diamond-square
heightmap = diamond_square_heightmap(n=n, min_height=min_height, max_height=max_height, noise=noise,
noise_factor=noise_factor)
elif algo == 3: # diamond-square (version 2)
heightmap = diamond_square_heightmap_2(n=n, min_height=min_height, max_height=max_height, noise=noise,
noise_factor=noise_factor)
elif algo == 4: # RBF interpolation
heightmap = rbf_heightmap(init_values=init_values, x=x, y=y, function=function, min_height=min_height,
max_height=max_height, dtype=dtype)
elif algo == 5: # 3D equation
heightmap = equation_heightmap(x=x, y=y, z=z, min_height=min_height, max_height=max_height, dtype=dtype)
elif algo == 6: # geospatial
from pyrobolearn.worlds.utils.heightmaps.geospatial import gdal_heightmap
heightmap = gdal_heightmap(filename=filename)
else:
raise NotImplementedError("The algo should be between 1 and 6 (see documentation).")
return heightmap
def generate_terrain(self, heightmap, filename):
@staticmethod
def generate_terrain(heightmap, filename, scale=600, smooth=True, verbose=True):
"""
Generate the terrain (obj) file and load it in the world.
Generate the terrain (obj) file; that is, create the OBJ file from the heightmap.
Args:
heightmap (np.array[W,H]): 2D heightmap.
filename (str): filename.
scale (float): scaling factor.
smooth (bool): if the normals should be smooth.
verbose (bool): if True, it will output information about the creation of the terrain.
Returns:
str: content of the OBJ file.
References:
[1] Wavefront .obj file (Wikipedia): https://en.wikipedia.org/wiki/Wavefront_.obj_file
"""
pass
obj = create_obj_from_heightmap(heightmap=heightmap, scale=scale, smooth=smooth, verbose=verbose,
filename=filename)
return obj
def load_stadium(self, scaling=1.):
"""
@@ -1065,7 +1113,7 @@ class World(object):
Returns:
int: unique id of the table
"""
table = self.sim.load_urdf('table/table.urdf', scale=scaling)
table = self.sim.load_urdf('table/table.urdf', position=position, scale=scaling)
self.movable_bodies[table] = 'table'
return table
@@ -1618,18 +1666,19 @@ if __name__ == '__main__':
sim = BulletSim()
# create world
world = BasicWorld(sim)
# world = World(sim)
# world = BasicWorld(sim)
world = World(sim)
# world.load_bot_lab()
# # load meshes
# world.load_mesh('meshes/terrain.obj',
# position=[0, 0, -2],
# orientation=[.707, 0, 0, .707],
# mass=0.,
# scale=(.1, .1, .1),
# # color=[1, 0, 0, 1],
# flags=1)
# load meshes
world.load_mesh('utils/terrains/terrain_map.obj',
position=[0, 0, -2],
orientation=[.707, 0, 0, .707],
mass=0.,
scale=(.1, .1, .1),
# color=[1, 0, 0, 1],
flags=1)
# world.load_visual_mesh('meshes/cube_color.dae', position=[0, 0, 1])
# world.load_mesh('bedroom.obj', [0, 0, 0], mass=0., color=[0.4, 0.4, 0.4, 1], flags=1) #, scale=(0.01, 0.01, 0.01))
# world.load_mesh('mtsthelens.obj', [0, 0, -8], mass=0., color=[0.2, 0.5, 0.2, 1], flags=1, scale=(0.01,0.01,0.01))
# world.load_mesh('meshes/terrain.obj', [0,0,0], mass=0., color=[1,1,1,1], flags=1)