Compare commits
No commits in common. "main" and "dev" have entirely different histories.
37 changed files with 933 additions and 1387 deletions
9
.gitignore
vendored
Normal file → Executable file
9
.gitignore
vendored
Normal file → Executable file
|
@ -1,3 +1,8 @@
|
|||
# Python
|
||||
**/__pycache__
|
||||
**/*.egg-info
|
||||
/dist
|
||||
**/__pycache__/
|
||||
|
||||
# Example
|
||||
**/example/*.jpg
|
||||
**/example/*.png
|
||||
**/example/*.svg
|
28
README.md
Normal file → Executable file
28
README.md
Normal file → Executable file
|
@ -1,28 +0,0 @@
|
|||
## Installation
|
||||
|
||||
```sh
|
||||
git clone pyskyline
|
||||
cd pyskyline
|
||||
pip install --user -e .
|
||||
```
|
||||
|
||||
## Demo
|
||||
|
||||
```sh
|
||||
cd demo
|
||||
python demo.py
|
||||
```
|
||||
|
||||
<div align="center">
|
||||
|
||||
<img src="images/fov.jpg" alt="simulated field of view" width="250"/>
|
||||
|
||||
**Fig.1** Simulated field of view for drone (blue dot).
|
||||
</div>
|
||||
|
||||
<div align="center">
|
||||
|
||||
<img src="images/heatmap.svg" alt="Computed heat map" width="450"/>
|
||||
|
||||
**Fig.2** Computed heat map in fourier domain.
|
||||
</div>
|
54
demo/demo.py
54
demo/demo.py
|
@ -1,54 +0,0 @@
|
|||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import cv2 as cv
|
||||
import os
|
||||
import pyskyline
|
||||
|
||||
import pyskyline.localization
|
||||
|
||||
FILE = 'terrain'
|
||||
SIZE = 256
|
||||
X = SIZE//2
|
||||
Y = SIZE//2+80
|
||||
|
||||
if __name__ == '__main__':
|
||||
if os.path.isfile(f'{FILE}_{SIZE}.npz'):
|
||||
print("Save detected, loading ...")
|
||||
terrain = pyskyline.Terrain.load(f'{FILE}_{SIZE}')
|
||||
else:
|
||||
print("No save detected, generating new terrain ...")
|
||||
terrain = pyskyline.Terrain.generate(size=SIZE)
|
||||
terrain.compute_all_skylines()
|
||||
terrain.save(f'{FILE}_{SIZE}')
|
||||
print(f"Done, terrain saved as {FILE}_{SIZE}.npz")
|
||||
|
||||
# Output elevation map
|
||||
cv.imwrite('elevation_map.jpg', terrain.dem.elevation)
|
||||
|
||||
# Output color map
|
||||
cv.imwrite('color_map.jpg', terrain.dem.color)
|
||||
|
||||
# Ouput field of view visualization
|
||||
cv.imwrite('fov.jpg', terrain.compute_fov(X, Y))
|
||||
|
||||
# Ouput skyline
|
||||
skyline = terrain.compute_skyline(X, Y)
|
||||
plt.figure()
|
||||
plt.grid(False)
|
||||
plt.plot(np.arange(-180,180,360/256),skyline)
|
||||
plt.title('Skyline')
|
||||
plt.savefig('skyline.svg')
|
||||
|
||||
# Generate heatmap
|
||||
skyline = np.roll(skyline, 8) # Add heading error
|
||||
skyline += np.random.normal(0,0.5, 256) # Add random noise
|
||||
mosse_correlation = pyskyline.MosseCorrelation(terrain, skyline)
|
||||
scoremap = mosse_correlation.generate_scoremap()
|
||||
|
||||
plt.figure()
|
||||
plt.grid(False)
|
||||
fig = plt.imshow(scoremap)
|
||||
plt.title('Score map')
|
||||
plt.colorbar(fig)
|
||||
plt.savefig('scoremap.svg')
|
||||
|
|
@ -1,134 +0,0 @@
|
|||
import numpy as np
|
||||
import scipy.signal as signal
|
||||
from scipy.fftpack import fft, fftshift, ifft
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.widgets import Slider
|
||||
|
||||
N = 256
|
||||
|
||||
def gaussian_filter1d(size,sigma):
|
||||
filter_range = np.linspace(-int(size/2),int(size/2),size)
|
||||
gaussian_filter = [1 / (sigma * np.sqrt(2*np.pi)) * np.exp(-x**2/(2*sigma**2)) for x in filter_range]
|
||||
return gaussian_filter
|
||||
|
||||
def generate_signal(N:int) -> np.ndarray:
|
||||
x = np.arange(1, N)
|
||||
y = np.zeros((N))
|
||||
for i in range(len(x)):
|
||||
y[i] = np.random.normal(scale=1) + (y[i-1] if i > 1 else 0)
|
||||
return np.convolve(y,gaussian_filter1d(N,1),'same')
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
np.random.seed(42)
|
||||
f_full = generate_signal(2048)
|
||||
x = np.arange(-180,180,360/N)
|
||||
f = f_full[1024:1024+N]
|
||||
h = f_full[1024:1024+N]
|
||||
g = signal.gaussian(N, std=10,sym=True)
|
||||
|
||||
F = fft(f)
|
||||
F_ = np.conjugate(F)
|
||||
G = fft(g)
|
||||
|
||||
K_ = (G*F_)/(F*F_)
|
||||
|
||||
H = fft(h)
|
||||
r = ifft(H*K_)
|
||||
|
||||
# ==========================================
|
||||
|
||||
fig, (ax1, ax2, ax3, ax4, ax5, ax6, ax7) = plt.subplots(7, 1, gridspec_kw={'height_ratios':[4,4,4,1,1,1,1]})
|
||||
|
||||
ax1.set_title('Terrain')
|
||||
plt_f, = ax1.plot(x,f)
|
||||
plt_h, = ax1.plot(x,h)
|
||||
|
||||
ax2.set_title('MOSSE response signal')
|
||||
ax2.set_ylim([0, 1.2])
|
||||
line_r = ax2.axvline(x=-N//2+np.argmax(abs(r)), color='r')
|
||||
plt_r, = ax2.plot(x,abs(r))
|
||||
plt_r2, = ax2.plot(x,abs(r))
|
||||
|
||||
ax3.set_title('Gaussian')
|
||||
plt_g, = ax3.plot(x,g)
|
||||
|
||||
ax1.set_xlim([-180,180])
|
||||
ax2.set_xlim([-180,180])
|
||||
ax3.set_xlim([-180,180])
|
||||
|
||||
slider1 = Slider(ax4, 'sigma', 0.3, 10, valinit=0.1)
|
||||
slider2 = Slider(ax5, 'shift', -N//2 , N//2, valinit=0, valstep=1)
|
||||
slider3 = Slider(ax6, 'seed', 0 , 50, valinit=0, valstep=1)
|
||||
slider4 = Slider(ax7, 'N', 128 , 1024, valinit=256, valstep=8)
|
||||
|
||||
sigma = 0.3
|
||||
shift = 0
|
||||
|
||||
def update():
|
||||
# K_ = (G*F_)/(F*F_)
|
||||
window = np.ones((N)) #signal.windows.hamming(N)
|
||||
H = fft(h*window)
|
||||
F = fft(f*window)
|
||||
R = H*G/F
|
||||
r = ifft(R)
|
||||
s = np.argmax(abs(r))
|
||||
r2 = np.copy(r)
|
||||
r2[s-5:s+5] = 0
|
||||
|
||||
plt_g.set_data(x,g)
|
||||
plt_r.set_data(x,abs(r))
|
||||
plt_r2.set_data(x,abs(r2))
|
||||
plt_h.set_data(x,h)
|
||||
plt_f.set_data(x,f)
|
||||
ax1.set_ylim([min(np.min(h),np.min(f))-1, max(np.max(h),np.max(f))+1])
|
||||
ax2.set_ylim([0, np.max(r)+0.2])
|
||||
line_r.set_xdata(round((np.argmax(abs(r))/N-0.5)*360))
|
||||
fig.canvas.draw_idle()
|
||||
|
||||
def update_sigma(val):
|
||||
global g, G, sigma, N
|
||||
sigma = val
|
||||
g = signal.gaussian(N, std=sigma,sym=True)
|
||||
G = fft(g)
|
||||
update()
|
||||
|
||||
def update_shift(val):
|
||||
global shift, H, h
|
||||
shift = -val
|
||||
h = f_full[1024+round(shift*(N/360)):1024+N+round(shift*(N/360))]
|
||||
noise = np.random.normal(0,0.5, N)
|
||||
h = h+noise
|
||||
update()
|
||||
|
||||
def update_seed(val):
|
||||
global f_full, f, h, F, H, F_, shift
|
||||
np.random.seed(val)
|
||||
f_full = generate_signal(2048)
|
||||
f = f_full[1024:1024+N]
|
||||
h = f_full[1024+round(shift*(N/360)):1024+N+round(shift*(N/360))]
|
||||
noise = np.random.normal(0,0.5, N)
|
||||
h = h+noise
|
||||
update()
|
||||
|
||||
def update_n(val):
|
||||
global g, G, N, f_full, f, h, F, H, F_, shift, x, sigma, slider2
|
||||
N = val
|
||||
x = np.arange(-180,180,360/N)
|
||||
g = signal.gaussian(N, std=sigma,sym=True)
|
||||
G = fft(g)
|
||||
f = f_full[1024:1024+N]
|
||||
h = f_full[1024+shift:1024+N+shift]
|
||||
noise = np.random.normal(0,0.5, N)
|
||||
h = h+noise
|
||||
|
||||
update()
|
||||
|
||||
|
||||
slider1.on_changed(update_sigma)
|
||||
slider2.on_changed(update_shift)
|
||||
slider3.on_changed(update_seed)
|
||||
slider4.on_changed(update_n)
|
||||
|
||||
plt.subplots_adjust(hspace=0.5)
|
||||
plt.show()
|
1
docs/.gitignore
vendored
Executable file
1
docs/.gitignore
vendored
Executable file
|
@ -0,0 +1 @@
|
|||
**/build
|
20
docs/Makefile
Executable file
20
docs/Makefile
Executable file
|
@ -0,0 +1,20 @@
|
|||
# Minimal makefile for Sphinx documentation
|
||||
#
|
||||
|
||||
# You can set these variables from the command line, and also
|
||||
# from the environment for the first two.
|
||||
SPHINXOPTS ?=
|
||||
SPHINXBUILD ?= sphinx-build
|
||||
SOURCEDIR = source
|
||||
BUILDDIR = build
|
||||
|
||||
# Put it first so that "make" without argument is like "make help".
|
||||
help:
|
||||
@$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
|
||||
|
||||
.PHONY: help Makefile
|
||||
|
||||
# Catch-all target: route all unknown targets to Sphinx using the new
|
||||
# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS).
|
||||
%: Makefile
|
||||
@$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
|
35
docs/make.bat
Executable file
35
docs/make.bat
Executable file
|
@ -0,0 +1,35 @@
|
|||
@ECHO OFF
|
||||
|
||||
pushd %~dp0
|
||||
|
||||
REM Command file for Sphinx documentation
|
||||
|
||||
if "%SPHINXBUILD%" == "" (
|
||||
set SPHINXBUILD=sphinx-build
|
||||
)
|
||||
set SOURCEDIR=source
|
||||
set BUILDDIR=build
|
||||
|
||||
%SPHINXBUILD% >NUL 2>NUL
|
||||
if errorlevel 9009 (
|
||||
echo.
|
||||
echo.The 'sphinx-build' command was not found. Make sure you have Sphinx
|
||||
echo.installed, then set the SPHINXBUILD environment variable to point
|
||||
echo.to the full path of the 'sphinx-build' executable. Alternatively you
|
||||
echo.may add the Sphinx directory to PATH.
|
||||
echo.
|
||||
echo.If you don't have Sphinx installed, grab it from
|
||||
echo.https://www.sphinx-doc.org/
|
||||
exit /b 1
|
||||
)
|
||||
|
||||
if "%1" == "" goto help
|
||||
|
||||
%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
|
||||
goto end
|
||||
|
||||
:help
|
||||
%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
|
||||
|
||||
:end
|
||||
popd
|
0
docs/requirements.txt
Executable file
0
docs/requirements.txt
Executable file
8
docs/source/api.rst
Executable file
8
docs/source/api.rst
Executable file
|
@ -0,0 +1,8 @@
|
|||
API
|
||||
===
|
||||
|
||||
.. autosummary::
|
||||
:toctree: api
|
||||
:recursive:
|
||||
|
||||
pyskyline
|
12
docs/source/api/pyskyline.digital_map.rst
Normal file
12
docs/source/api/pyskyline.digital_map.rst
Normal file
|
@ -0,0 +1,12 @@
|
|||
pyskyline.digital\_map
|
||||
======================
|
||||
|
||||
.. automodule:: pyskyline.digital_map
|
||||
|
||||
|
||||
.. rubric:: Classes
|
||||
|
||||
.. autosummary::
|
||||
|
||||
DigitalMap
|
||||
|
16
docs/source/api/pyskyline.raycast.rst
Normal file
16
docs/source/api/pyskyline.raycast.rst
Normal file
|
@ -0,0 +1,16 @@
|
|||
pyskyline.raycast
|
||||
=================
|
||||
|
||||
.. automodule:: pyskyline.raycast
|
||||
|
||||
|
||||
.. rubric:: Functions
|
||||
|
||||
.. autosummary::
|
||||
|
||||
bresenham_line
|
||||
compute_fov
|
||||
compute_skyline
|
||||
get_highest_angle
|
||||
mp_compute_skyline
|
||||
|
15
docs/source/api/pyskyline.rst
Normal file
15
docs/source/api/pyskyline.rst
Normal file
|
@ -0,0 +1,15 @@
|
|||
pyskyline
|
||||
=========
|
||||
|
||||
.. automodule:: pyskyline
|
||||
|
||||
|
||||
.. rubric:: Modules
|
||||
|
||||
.. autosummary::
|
||||
:toctree:
|
||||
:recursive:
|
||||
|
||||
digital_map
|
||||
raycast
|
||||
terrain
|
12
docs/source/api/pyskyline.terrain.rst
Normal file
12
docs/source/api/pyskyline.terrain.rst
Normal file
|
@ -0,0 +1,12 @@
|
|||
pyskyline.terrain
|
||||
=================
|
||||
|
||||
.. automodule:: pyskyline.terrain
|
||||
|
||||
|
||||
.. rubric:: Classes
|
||||
|
||||
.. autosummary::
|
||||
|
||||
Terrain
|
||||
|
35
docs/source/conf.py
Executable file
35
docs/source/conf.py
Executable file
|
@ -0,0 +1,35 @@
|
|||
import os
|
||||
import sys
|
||||
sys.path.insert(0, os.path.abspath('..'))
|
||||
|
||||
# Configuration file for the Sphinx documentation builder.
|
||||
#
|
||||
# For the full list of built-in configuration values, see the documentation:
|
||||
# https://www.sphinx-doc.org/en/master/usage/configuration.html
|
||||
|
||||
# -- Project information -----------------------------------------------------
|
||||
# https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information
|
||||
|
||||
project = 'pyskyline'
|
||||
copyright = '2024, Alexandre Foucher'
|
||||
author = 'Alexandre Foucher'
|
||||
release = 'alpha0.1'
|
||||
|
||||
# -- General configuration ---------------------------------------------------
|
||||
# https://www.sphinx-doc.org/en/master/usage/configuration.html#general-configuration
|
||||
|
||||
extensions = [
|
||||
'sphinx.ext.duration',
|
||||
'sphinx.ext.doctest',
|
||||
'sphinx.ext.autodoc',
|
||||
'sphinx.ext.autosummary',
|
||||
]
|
||||
|
||||
templates_path = ['_templates']
|
||||
exclude_patterns = []
|
||||
|
||||
# -- Options for HTML output -------------------------------------------------
|
||||
# https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output
|
||||
|
||||
html_theme = 'alabaster'
|
||||
html_static_path = ['_static']
|
18
docs/source/index.rst
Executable file
18
docs/source/index.rst
Executable file
|
@ -0,0 +1,18 @@
|
|||
.. pyskyline documentation master file, created by
|
||||
sphinx-quickstart on Tue Aug 27 09:34:56 2024.
|
||||
You can adapt this file completely to your liking, but it should at least
|
||||
contain the root `toctree` directive.
|
||||
|
||||
pyskyline documentation
|
||||
=======================
|
||||
|
||||
Add your content using ``reStructuredText`` syntax. See the
|
||||
`reStructuredText <https://www.sphinx-doc.org/en/master/usage/restructuredtext/index.html>`_
|
||||
documentation for details.
|
||||
|
||||
|
||||
.. toctree::
|
||||
:maxdepth: 2
|
||||
:caption: Contents:
|
||||
|
||||
api
|
74
evaluate/eval.py
Normal file
74
evaluate/eval.py
Normal file
|
@ -0,0 +1,74 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import math
|
||||
import cv2 as cv
|
||||
import pyskyline
|
||||
import numpy as np
|
||||
import argparse
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.widgets import Slider
|
||||
|
||||
def show_result(terrain, skyline, coord, scoremap):
|
||||
fig = plt.figure()
|
||||
gs = fig.add_gridspec(2,3)
|
||||
fig.suptitle("Mosse correlation", fontsize=16)
|
||||
ax1 = fig.add_subplot(gs[0, 0])
|
||||
ax2 = fig.add_subplot(gs[0, 1])
|
||||
ax3 = fig.add_subplot(gs[0, 2])
|
||||
ax4 = fig.add_subplot(gs[1, :])
|
||||
|
||||
im1 = ax1.imshow(scoremap, cmap='hot', interpolation='nearest')
|
||||
ax1.set_title('Correlation map')
|
||||
cbar1 = ax1.figure.colorbar(im1)
|
||||
cbar1.ax.set_ylabel("Correlation score", rotation=-90, va="bottom")
|
||||
|
||||
rgb = cv.cvtColor(terrain.rgb(), cv.COLOR_BGR2RGB)
|
||||
rgb = cv.circle(rgb, coord, 2, (255,0,0), -1)
|
||||
ax2.imshow(rgb)
|
||||
ax2.set_title('Colored terrain')
|
||||
|
||||
im2 = ax3.imshow((terrain.grayscale().astype(float) - 135) * (40.0/120.0))
|
||||
ax3.set_title('DEM')
|
||||
cbar = ax1.figure.colorbar(im2)
|
||||
cbar.ax.set_ylabel("Altitude", rotation=-90, va="bottom")
|
||||
|
||||
x = np.linspace(-180,180,skyline.shape[0])
|
||||
ax4.plot(x,skyline,label="Agent vision")
|
||||
ax4.plot(x,terrain.get_skyline(coord[0], coord[1]), label="DEM data")
|
||||
ax4.legend()
|
||||
|
||||
plt.show()
|
||||
|
||||
def eval_score(correlation_map, coord, scale, alpha=10):
|
||||
alpha = alpha / scale
|
||||
score = 0
|
||||
h,w = correlation_map.shape
|
||||
x,y = coord
|
||||
for j in range(h):
|
||||
for i in range(w):
|
||||
dist = math.sqrt((x-i)**2+(y-j)**2)
|
||||
score += correlation_map[j,i] * 1 #(math.pow(0.5,math.pow(dist/alpha,2)-1)-1)
|
||||
return score
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser()
|
||||
|
||||
parser.add_argument("--size", help="Terrain size (default=128)", type=int, default=128)
|
||||
parser.add_argument("--seed", help="Terrain seed (If not set seed is random)", type=int)
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
seed = np.random.randint(0,1024) if args.seed is None else args.seed
|
||||
terrain = pyskyline.Terrain.generate(size=args.size,seed=seed,scale=8.0,max_altitude=40.0)
|
||||
|
||||
terrain.precompute_all_skylines()
|
||||
|
||||
x, y = terrain.get_random_water_coordinate()
|
||||
skyline = terrain.get_skyline(x, y)
|
||||
angle = np.random.randint(1,40)
|
||||
skyline = np.roll(skyline, angle)
|
||||
correlation_map = pyskyline.MosseCorrelation.process(terrain, skyline)
|
||||
score = eval_score(correlation_map,(x,y), terrain._scale,100)
|
||||
score = score / (2 * terrain.water_tile_count()) + 0.5
|
||||
print(f"{score:0.2f} with angle of {angle}")
|
||||
show_result(terrain, skyline, (x,y), correlation_map)
|
44
example/demo.py
Normal file
44
example/demo.py
Normal file
|
@ -0,0 +1,44 @@
|
|||
import pyskyline
|
||||
import random
|
||||
|
||||
terrain = pyskyline.Terrain.generate(
|
||||
seed=10,
|
||||
size=512,
|
||||
scale=4.0,
|
||||
max_altitude=40.0
|
||||
)
|
||||
|
||||
terrain.precompute_all_skylines()
|
||||
|
||||
while True:
|
||||
x = random.randint(0, 511)
|
||||
y = random.randint(0, 511)
|
||||
if terrain.is_water(x,y): break
|
||||
|
||||
skyline = terrain.get_skyline(x, y)
|
||||
print(f"({x}, {y})")
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
plt.figure()
|
||||
plt.grid(False)
|
||||
plt.ylim(0,20)
|
||||
plt.plot(np.arange(0,360,360/256), np.rad2deg(skyline))
|
||||
plt.title('Skyline')
|
||||
|
||||
import cv2 as cv
|
||||
|
||||
cv.imwrite('elevation.jpg', terrain.grayscale())
|
||||
cv.imwrite('color.jpg', terrain.rgb())
|
||||
|
||||
skyline = np.roll(skyline, 21) # Add heading error
|
||||
# skyline += np.random.normal(0,0.01, 256) # Add random noise
|
||||
|
||||
res = pyskyline.MosseCorrelation.process(terrain, skyline)
|
||||
|
||||
plt.figure()
|
||||
plt.imshow(res, cmap='hot', interpolation='nearest')
|
||||
plt.title('Scoremap')
|
||||
|
||||
plt.show()
|
130
example/mosse_viz.py
Executable file
130
example/mosse_viz.py
Executable file
|
@ -0,0 +1,130 @@
|
|||
import numpy as np
|
||||
import scipy.signal as signal
|
||||
from scipy.fftpack import fft, ifft
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.widgets import Slider
|
||||
|
||||
def gaussian_filter1d(size,sigma):
|
||||
filter_range = np.linspace(-int(size/2),int(size/2),size)
|
||||
gaussian_filter = [1 / (sigma * np.sqrt(2*np.pi)) * np.exp(-x**2/(2*sigma**2)) for x in filter_range]
|
||||
return gaussian_filter
|
||||
|
||||
def generate_signal(N:int) -> np.ndarray:
|
||||
x = np.arange(1, N)
|
||||
y = np.zeros((N))
|
||||
for i in range(len(x)):
|
||||
y[i] = np.random.normal(scale=1) + (y[i-1] if i > 1 else 0)
|
||||
return np.convolve(y,gaussian_filter1d(N,1),'same')
|
||||
|
||||
class UI:
|
||||
def __init__(self) -> None:
|
||||
self._size_F = 256
|
||||
self._size_H = 256
|
||||
self._sigma = 1.1
|
||||
self._seed = 0
|
||||
self._shift = 0
|
||||
self._fig, self._axs = self.init_ui()
|
||||
|
||||
self.update()
|
||||
|
||||
def init_ui(self) -> None:
|
||||
fig, (ax1, ax2, ax3, ax4, ax5, ax6, ax7, ax8) = plt.subplots(8, 1, gridspec_kw={'height_ratios':[4,4,4,1,1,1,1,1]})
|
||||
|
||||
ax1.set_title('Terrain')
|
||||
self.plt_f, = ax1.plot([])
|
||||
self.plt_h, = ax1.plot([])
|
||||
|
||||
ax2.set_title('MOSSE response signal')
|
||||
ax2.set_ylim([0, 1.2])
|
||||
self.line_r = ax2.axvline()
|
||||
self.plt_r, = ax2.plot([])
|
||||
self.plt_r2, = ax2.plot([])
|
||||
|
||||
ax3.set_title('Gaussian')
|
||||
self.plt_g, = ax3.plot([])
|
||||
|
||||
ax1.set_xlim([-180,180])
|
||||
ax2.set_xlim([-180,180])
|
||||
ax3.set_xlim([-180,180])
|
||||
|
||||
self._slider1 = Slider(ax4, 'sigma', 0.1, 5, valinit=self._sigma)
|
||||
self._slider2 = Slider(ax5, 'shift', -180, 180, valinit=self._shift, valstep=1)
|
||||
self._slider3 = Slider(ax6, 'seed', 0, 50, valinit=self._seed, valstep=1)
|
||||
self._slider4 = Slider(ax7, 'Size f', 64, 1024, valinit=self._size_F, valstep=8)
|
||||
self._slider5 = Slider(ax8, 'Size h', 64, 1024, valinit=self._size_H, valstep=8)
|
||||
|
||||
self._slider1.on_changed(self.update_sigma)
|
||||
self._slider2.on_changed(self.update_shift)
|
||||
self._slider3.on_changed(self.update_seed)
|
||||
self._slider4.on_changed(self.update_size_f)
|
||||
self._slider5.on_changed(self.update_size_h)
|
||||
|
||||
fig.subplots_adjust(hspace=0.75)
|
||||
|
||||
return fig, [ax1, ax2, ax3, ax4, ax5, ax6, ax7, ax8]
|
||||
|
||||
def update(self):
|
||||
np.random.seed(self._seed)
|
||||
g = signal.windows.gaussian(self._size_F, std=self._sigma,sym=True)
|
||||
f = generate_signal(self._size_F) * signal.windows.hamming(self._size_F)
|
||||
h = np.zeros((self._size_F))
|
||||
s = self._size_F//2-self._size_H//2
|
||||
h[s:s+self._size_H] = (f[s:s+self._size_H] + np.random.normal(0,0.5, self._size_H)) * signal.windows.hamming(self._size_H)
|
||||
h = np.roll(h,int(self._shift/180*self._size_F//2))
|
||||
F = fft(f)
|
||||
G = fft(g)
|
||||
H = fft(h)
|
||||
R = H*G/F
|
||||
r = ifft(R)
|
||||
s = np.argmax(abs(r))
|
||||
r2 = np.copy(r)
|
||||
r2[s-5:s+5] = 0
|
||||
|
||||
x = np.linspace(-180,180,self._size_F)
|
||||
self.plt_g.set_data(x, g)
|
||||
self.plt_r.set_data(x,abs(r))
|
||||
self.plt_r2.set_data(x,abs(r2))
|
||||
self.plt_f.set_data(x,f)
|
||||
self.plt_h.set_data(x,h)
|
||||
self._axs[0].set_ylim([min(np.min(h),np.min(f))-1, max(np.max(h),np.max(f))+1])
|
||||
self._axs[1].set_ylim([0, np.max(r)+0.2])
|
||||
self._axs[2].set_ylim([0, np.max(g)*1.1])
|
||||
self.line_r.set_xdata([round((np.argmax(abs(r))/self._size_F-0.5)*360)])
|
||||
self._fig.canvas.draw_idle()
|
||||
|
||||
def update_sigma(self, val):
|
||||
self._sigma = val
|
||||
self.update()
|
||||
|
||||
def update_size_f(self, val):
|
||||
if val < self._size_H:
|
||||
self._slider4.eventson = False
|
||||
self._slider4.set_val(max(val, self._size_H))
|
||||
self._slider4.eventson = True
|
||||
|
||||
self._size_F = max(val, self._size_H)
|
||||
self.update()
|
||||
|
||||
def update_size_h(self, val):
|
||||
if val > self._size_F:
|
||||
self._slider5.eventson = False
|
||||
self._slider5.set_val(min(val, self._size_F))
|
||||
self._slider5.eventson = True
|
||||
|
||||
self._size_H = min(val, self._size_F)
|
||||
self.update()
|
||||
|
||||
def update_seed(self, val):
|
||||
self._seed = val
|
||||
self.update()
|
||||
|
||||
def update_shift(self, val):
|
||||
self._shift = val
|
||||
self.update()
|
||||
|
||||
def show(self) -> None:
|
||||
plt.show()
|
||||
|
||||
if __name__ == '__main__':
|
||||
ui = UI()
|
||||
ui.show()
|
BIN
images/fov.jpg
BIN
images/fov.jpg
Binary file not shown.
Before Width: | Height: | Size: 24 KiB |
File diff suppressed because one or more lines are too long
Before Width: | Height: | Size: 98 KiB |
4
pyproject.toml
Normal file → Executable file
4
pyproject.toml
Normal file → Executable file
|
@ -3,9 +3,9 @@ requires = ["setuptools>=61.0"]
|
|||
build-backend = "setuptools.build_meta"
|
||||
[project]
|
||||
name = "pyskyline"
|
||||
version = "0.0.1"
|
||||
version = "0.1"
|
||||
authors = [
|
||||
{ name="Alexandre FOUCHER", email="foucher@univ-ubs.fr" },
|
||||
{ name="Alexandre FOUCHER", email="alexandre.foucher@univ-ubs.fr" },
|
||||
]
|
||||
description = "A small example package"
|
||||
readme = "README.md"
|
||||
|
|
|
@ -1,4 +1,4 @@
|
|||
perlin-numpy @ git+https://github.com/pvigier/perlin-numpy
|
||||
numpy
|
||||
scipy
|
||||
opencv-python
|
||||
noise==1.2.2
|
||||
scipy==1.14.1
|
||||
matplotlib
|
||||
pyqt5
|
Binary file not shown.
Before Width: | Height: | Size: 12 KiB |
6
src/cppskyline/.gitignore
vendored
Executable file
6
src/cppskyline/.gitignore
vendored
Executable file
|
@ -0,0 +1,6 @@
|
|||
# Build files
|
||||
**/lib
|
||||
**/CMakeFiles
|
||||
CMakeCache.txt
|
||||
cmake_install.cmake
|
||||
Makefile
|
30
src/cppskyline/CMakeLists.txt
Executable file
30
src/cppskyline/CMakeLists.txt
Executable file
|
@ -0,0 +1,30 @@
|
|||
# Specify the minimum version.
|
||||
cmake_minimum_required(VERSION 3.9)
|
||||
|
||||
# Specify the project info.
|
||||
project(convert VERSION 1.0.0 LANGUAGES C CXX)
|
||||
|
||||
find_package(OpenMP REQUIRED)
|
||||
if (OPENMP_FOUND)
|
||||
set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
|
||||
set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
|
||||
set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
|
||||
endif()
|
||||
|
||||
set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
|
||||
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
|
||||
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
|
||||
|
||||
# set(CMAKE_CXX_FLAGS_RELEASE "-Ofast")
|
||||
|
||||
file(GLOB_RECURSE SRC_FILES
|
||||
${PROJECT_SOURCE_DIR}/src/*.cpp
|
||||
${PROJECT_SOURCE_DIR}/src/*.h
|
||||
)
|
||||
|
||||
include_directories(include)
|
||||
|
||||
# Declare the library target.
|
||||
add_library(${PROJECT_NAME} SHARED ${SRC_FILES})
|
||||
|
||||
target_link_libraries(${PROJECT_NAME} fftw3)
|
1
src/cppskyline/README.md
Executable file
1
src/cppskyline/README.md
Executable file
|
@ -0,0 +1 @@
|
|||
sudo apt-get install fftw3 fftw3-dev pkg-config
|
21
src/cppskyline/include/MosseCorrelation.hpp
Normal file
21
src/cppskyline/include/MosseCorrelation.hpp
Normal file
|
@ -0,0 +1,21 @@
|
|||
# ifndef MOSSE_CORRELATION_HPP
|
||||
# define MOSSE_CORRELATION_HPP
|
||||
|
||||
#include <cstddef>
|
||||
#include <fftw3.h>
|
||||
|
||||
#include "Terrain.hpp"
|
||||
|
||||
namespace skyline
|
||||
{
|
||||
// fftw_complex *skyline::fft(float *array, std::size_t N);
|
||||
fftw_complex *fft(float *array, std::size_t N);
|
||||
fftw_complex *ifft(fftw_complex *in, std::size_t N);
|
||||
float *abs(fftw_complex *in, std::size_t N);
|
||||
void multiply_complex(fftw_complex *a, fftw_complex *b, std::size_t N);
|
||||
void divide_complex(fftw_complex *a, fftw_complex *b, std::size_t N);
|
||||
float *gaussian(std::size_t N, float std);
|
||||
float *process(skyline::Terrain *terrain, float *f);
|
||||
}
|
||||
|
||||
# endif /* MOSSE_CORRELATION_HPP */
|
35
src/cppskyline/include/Terrain.hpp
Normal file
35
src/cppskyline/include/Terrain.hpp
Normal file
|
@ -0,0 +1,35 @@
|
|||
# ifndef TERRAIN_HPP
|
||||
# define TERRAIN_HPP
|
||||
|
||||
# include <cstdint>
|
||||
# include <map>
|
||||
|
||||
namespace skyline
|
||||
{
|
||||
class Terrain
|
||||
{
|
||||
private:
|
||||
std::size_t m_size;
|
||||
std::uint8_t *m_dem;
|
||||
std::uint8_t m_ocean_height;
|
||||
float m_altitude_ratio;
|
||||
float m_scale;
|
||||
std::uint16_t m_raycount;
|
||||
float m_raydist;
|
||||
float m_raystep;
|
||||
std::map<std::uint32_t, float*> m_skylines;
|
||||
|
||||
float *compute_skyline(std::int32_t x, std::int32_t y);
|
||||
|
||||
public:
|
||||
Terrain(std::size_t size, std::uint8_t *dem, std::uint8_t ocena_height, float max_altitude, float scale, std::uint16_t raycount, float raydist);
|
||||
~Terrain();
|
||||
std::size_t get_size();
|
||||
float *get_skyline(std::uint32_t x, std::uint32_t y);
|
||||
bool is_water(std::int32_t x, std::int32_t y);
|
||||
void precompute_all_skylines();
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* TERRAIN_HPP */
|
119
src/cppskyline/src/MosseCorrelation.cpp
Normal file
119
src/cppskyline/src/MosseCorrelation.cpp
Normal file
|
@ -0,0 +1,119 @@
|
|||
#include "MosseCorrelation.hpp"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#define GAUSSIAN_STD 2.25
|
||||
|
||||
fftw_complex *skyline::fft(float *array, std::size_t N)
|
||||
{
|
||||
fftw_complex *in = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * N);
|
||||
fftw_complex *out = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * N);
|
||||
fftw_plan p = fftw_plan_dft_1d(N, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
|
||||
|
||||
for(std::size_t i=0; i < N; i++) {
|
||||
in[i][0] = array[i];
|
||||
}
|
||||
|
||||
fftw_execute(p);
|
||||
|
||||
fftw_destroy_plan(p);
|
||||
fftw_free(in);
|
||||
return out;
|
||||
}
|
||||
|
||||
fftw_complex *skyline::ifft(fftw_complex *in, std::size_t N)
|
||||
{
|
||||
fftw_complex *out = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * N);
|
||||
fftw_plan p = fftw_plan_dft_1d(N, in, out, FFTW_BACKWARD, FFTW_ESTIMATE);
|
||||
|
||||
fftw_execute(p);
|
||||
|
||||
fftw_destroy_plan(p);
|
||||
fftw_free(in);
|
||||
return out;
|
||||
}
|
||||
|
||||
float *skyline::abs(fftw_complex *in, std::size_t N)
|
||||
{
|
||||
float *array = new float[N];
|
||||
|
||||
for(std::size_t i=0; i < N; i++) {
|
||||
array[i] = std::sqrt( in[i][0] * in[i][0] + in[i][1] * in[i][1]);
|
||||
}
|
||||
|
||||
return array;
|
||||
}
|
||||
|
||||
void skyline::multiply_complex(fftw_complex *a, fftw_complex *b, std::size_t N)
|
||||
{
|
||||
double r,i;
|
||||
for(std::size_t i=0; i < N; i++) {
|
||||
r = a[i][0] * b[i][0] - a[i][1] * b[i][1];
|
||||
i = a[i][0] * b[i][1] + b[i][0] * a[i][1];
|
||||
a[i][0] = r;
|
||||
a[i][1] = i;
|
||||
}
|
||||
}
|
||||
|
||||
void skyline::divide_complex(fftw_complex *a, fftw_complex *b, std::size_t N)
|
||||
{
|
||||
double r,i, denum;
|
||||
for(std::size_t i=0; i < N; i++) {
|
||||
denum = b[i][0] * b[i][0] + b[i][1] * b[i][1];
|
||||
r = (a[i][0] * b[i][0] + a[i][1] * b[i][1]) / (denum);
|
||||
i = (a[i][1] * b[i][0] - a[i][0] * b[i][1]) / (denum);
|
||||
a[i][0] = r;
|
||||
a[i][1] = i;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
float *skyline::gaussian(std::size_t N, float std)
|
||||
{
|
||||
float *array = new float[N];
|
||||
float half_m = (N - 1.0) / 2.0;
|
||||
float sig2 = 2 * std * std;
|
||||
|
||||
for(std::size_t i=0; i < N; i++) {
|
||||
array[i] = std::exp(-std::pow( (i-half_m), 2) / sig2);
|
||||
}
|
||||
|
||||
return array;
|
||||
};
|
||||
|
||||
float *skyline::process(skyline::Terrain *terrain, float *f) {
|
||||
size_t N = terrain->get_size();
|
||||
float *g = gaussian(N, GAUSSIAN_STD);
|
||||
fftw_complex *G = fft(g, N);
|
||||
fftw_complex *F = fft(f, N);
|
||||
float *scoremap = new float[N*N];
|
||||
|
||||
#pragma omp parallel for collapse(2)
|
||||
for(size_t x=0; x < N; x++)
|
||||
{
|
||||
for(size_t y=0; y < N; y++)
|
||||
{
|
||||
if(terrain->is_water(x, y))
|
||||
{
|
||||
fftw_complex *H = fft(terrain->get_skyline(x, y), N);
|
||||
multiply_complex(H, G, N);
|
||||
divide_complex(H, F, N);
|
||||
H = ifft(H, N);
|
||||
float *r = skyline::abs(H, N);
|
||||
|
||||
// TODO compute score
|
||||
|
||||
fftw_free(H);
|
||||
free(r);
|
||||
} else {
|
||||
scoremap[y * N + x] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
free(g);
|
||||
fftw_free(G);
|
||||
fftw_free(F);
|
||||
|
||||
return scoremap;
|
||||
};
|
141
src/cppskyline/src/Terrain.cpp
Normal file
141
src/cppskyline/src/Terrain.cpp
Normal file
|
@ -0,0 +1,141 @@
|
|||
# include "Terrain.hpp"
|
||||
# include <math.h>
|
||||
# include <iostream>
|
||||
# include <vector>
|
||||
|
||||
// ============================================================================
|
||||
// P R I V A T E
|
||||
// ============================================================================
|
||||
|
||||
float *skyline::Terrain::compute_skyline(std::int32_t x, std::int32_t y)
|
||||
{
|
||||
float *skyline = new float[this->m_raycount];
|
||||
|
||||
for(int i=0; i < this->m_raycount; i++){
|
||||
int x0 = x;
|
||||
int y0 = y;
|
||||
int x1 = x0 + cos(i * this->m_raystep) * this->m_raydist;
|
||||
int y1 = y0 + sin(i * this->m_raystep) * this->m_raydist;
|
||||
int dx = abs(x1-x0), sx = x0<x1 ? 1 : -1;
|
||||
int dy = -abs(y1-y0), sy = y0<y1 ? 1 : -1;
|
||||
int err = dx+dy, e2;
|
||||
|
||||
std::uint8_t height = 0;
|
||||
float max_angle = 0;
|
||||
float dist, angle;
|
||||
|
||||
while( (x0 != x1 or y0 != y1) and (x0 < this->m_size && x0 >= 0 && y0 < this->m_size && y0 >= 0) ){
|
||||
if (this->m_dem[y0 * this->m_size + x0] > height) {
|
||||
height = this->m_dem[y0 * this->m_size + x0];
|
||||
dist = sqrt(pow(x0-x,2)+pow(y0-y,2)) * this->m_scale;
|
||||
angle = atan((height-this->m_ocean_height)*this->m_altitude_ratio/dist);
|
||||
|
||||
if(angle > max_angle){
|
||||
max_angle = angle;
|
||||
}
|
||||
}
|
||||
|
||||
e2 = 2*err;
|
||||
if (e2 >= dy) { err += dy; x0 += sx; } /* e_xy+e_x > 0 */
|
||||
if (e2 <= dx) { err += dx; y0 += sy; } /* e_xy+e_y < 0 */
|
||||
}
|
||||
|
||||
skyline[i] = max_angle;
|
||||
}
|
||||
|
||||
return skyline;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// P U B L I C
|
||||
// ============================================================================
|
||||
|
||||
skyline::Terrain::Terrain(std::size_t size, std::uint8_t *dem, std::uint8_t ocean_height,
|
||||
float max_altitude, float scale, std::uint16_t raycount, float raydist)
|
||||
{
|
||||
this->m_size = size;
|
||||
this->m_dem = dem;
|
||||
this->m_ocean_height = ocean_height;
|
||||
this->m_altitude_ratio = max_altitude / 255.0;
|
||||
this->m_scale = scale;
|
||||
this->m_raycount = raycount;
|
||||
this->m_raydist = raydist / scale;
|
||||
this->m_raystep = 2 * M_PI / (raycount - 1);
|
||||
}
|
||||
|
||||
skyline::Terrain::~Terrain()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
std::size_t skyline::Terrain::get_size()
|
||||
{
|
||||
return this->m_size;
|
||||
}
|
||||
|
||||
bool skyline::Terrain::is_water(int32_t x, int32_t y)
|
||||
{
|
||||
return this->m_dem[y * this->m_size + x] <= this->m_ocean_height;
|
||||
}
|
||||
|
||||
float *skyline::Terrain::get_skyline(std::uint32_t x, std::uint32_t y)
|
||||
{
|
||||
std::uint64_t id = (y << 16) | x;
|
||||
|
||||
// Check if skyline is already computed
|
||||
if (this->is_water(x,y))
|
||||
{
|
||||
if (this->m_skylines.find(id) == this->m_skylines.end()) {
|
||||
float *skyline = this->compute_skyline(x, y);
|
||||
this->m_skylines.insert({id, skyline});
|
||||
return skyline;
|
||||
} else {
|
||||
return this->m_skylines.at(id);
|
||||
}
|
||||
}
|
||||
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
void skyline::Terrain::precompute_all_skylines()
|
||||
{
|
||||
std::vector<std::uint32_t> sharedList;
|
||||
|
||||
#pragma omp parallel for collapse(2)
|
||||
for( int x=0; x<this->m_size; x++)
|
||||
{
|
||||
for( int y=0; y<this->m_size; y++)
|
||||
{
|
||||
if (this->is_water(x,y))
|
||||
{
|
||||
#pragma omp critical
|
||||
sharedList.push_back(y << 16 | x);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#pragma omp parallel for
|
||||
for( int i=0; i<sharedList.size(); i++)
|
||||
{
|
||||
std::uint32_t id = sharedList.at(i);
|
||||
std::uint16_t y = (0xFFFF0000 & id) >> 16;
|
||||
std::uint16_t x = (0x0000FFFF & id);
|
||||
|
||||
float *skyline = this->compute_skyline(x, y);
|
||||
|
||||
#pragma omp critical
|
||||
this->m_skylines.insert({id, skyline});
|
||||
}
|
||||
}
|
||||
|
||||
extern "C" {
|
||||
skyline::Terrain *Terrain_new(std::uint32_t size, std::uint8_t *dem, std::uint8_t ocean_height,
|
||||
float max_altitude, float scale, std::uint16_t raycount, float raydist)
|
||||
{
|
||||
return new skyline::Terrain(size, dem, ocean_height, max_altitude, scale, raycount, raydist);
|
||||
}
|
||||
void Terrain_free(skyline::Terrain *terrain){delete terrain;}
|
||||
float *Terrain_get_skyline(skyline::Terrain *terrain, std::int32_t x, std::int32_t y){ return terrain->get_skyline(x, y); }
|
||||
void Terrain_precompute_all_skylines(skyline::Terrain *terrain){ terrain->precompute_all_skylines(); }
|
||||
bool Terrain_is_water(skyline::Terrain *terrain, std::int32_t x, std::int32_t y){ return terrain->is_water(x, y); }
|
||||
};
|
7
src/pyskyline/__init__.py
Normal file → Executable file
7
src/pyskyline/__init__.py
Normal file → Executable file
|
@ -1,4 +1,5 @@
|
|||
from .digital_map import DigitalMap
|
||||
from .raycast import bresenham_line, get_highest_angle
|
||||
DEFAULT_RAYCOUNT = 256
|
||||
DEFAULT_RAYDIST = 1000.0
|
||||
|
||||
from .terrain import Terrain
|
||||
from .localization.mosse_correlation import MosseCorrelation
|
||||
from .mosse_correlation import MosseCorrelation
|
|
@ -1,77 +0,0 @@
|
|||
import os
|
||||
import numpy as np
|
||||
import cv2 as cv
|
||||
import perlin_numpy as perlin
|
||||
|
||||
class DigitalMap:
|
||||
DEFAULT_MASK = os.path.dirname(__file__) + '/../../resources/mask.jpg'
|
||||
DEFAULT_LAYERS = {
|
||||
-1 : (169, 166, 97), # Water
|
||||
18 : (175, 214, 238), # Sand
|
||||
36 : ( 34, 139, 34), # Grass
|
||||
82 : ( 20, 100, 20) # Tree
|
||||
}
|
||||
|
||||
def __init__(self, seed:int, size:int, height:float, scale:float) -> None:
|
||||
self.height = height
|
||||
self.scale = scale
|
||||
self.seed = seed
|
||||
self.size = size
|
||||
self.elevation : 'np.ndarray|None' = None
|
||||
self.color : 'np.ndarray|None' = None
|
||||
|
||||
def generate_elevation_map(self, mask_path:str) -> None:
|
||||
""" Generate grayscale elevation map from 2D perlin noise
|
||||
|
||||
Args:
|
||||
mask_path (str): Terrain grayscale mask path.
|
||||
"""
|
||||
np.random.seed(self.seed)
|
||||
perlin_noise = perlin.generate_fractal_noise_2d(
|
||||
shape = (512,)*2,
|
||||
res = (2**3,)*2,
|
||||
octaves = 5
|
||||
)
|
||||
|
||||
# Set noise in [0;1]
|
||||
p_min = np.min(perlin_noise)
|
||||
p_max = np.max(perlin_noise)
|
||||
perlin_noise = (perlin_noise - p_min) / (p_max - p_min)
|
||||
|
||||
if self.size != 512:
|
||||
perlin_noise = cv.resize(perlin_noise, (self.size,)*2, interpolation=cv.INTER_CUBIC)
|
||||
|
||||
# Open and rescale mask
|
||||
mask = cv.imread(mask_path, cv.IMREAD_GRAYSCALE)
|
||||
mask = cv.resize(mask, (self.size,)*2, interpolation=cv.INTER_CUBIC)
|
||||
|
||||
OCEAN_HEIGHT = 30
|
||||
elevation_map = perlin_noise * mask
|
||||
elevation_map = np.clip(elevation_map, OCEAN_HEIGHT, 255)
|
||||
elevation_map = (elevation_map-OCEAN_HEIGHT) / (255-OCEAN_HEIGHT) * 255
|
||||
|
||||
self.elevation = elevation_map.astype(dtype=np.uint8)
|
||||
|
||||
def generate_color_map(self, layers:'dict|None'=None) -> None:
|
||||
""" Generate RGB colored map based on elevation map.
|
||||
|
||||
Args:
|
||||
layers (dict): Layer dictionary where the key is the height and value the color in BGR.
|
||||
"""
|
||||
if layers is None:
|
||||
layers = self.DEFAULT_LAYERS
|
||||
|
||||
color_map = np.full((self.size, self.size, 3), layers[-1], dtype=np.uint8)
|
||||
|
||||
for height, color in list(layers.items())[1:]:
|
||||
mask = self.elevation > height
|
||||
color_map[mask] = color
|
||||
|
||||
self.color = color_map
|
||||
|
||||
@staticmethod
|
||||
def generate(seed:int, size:int, height:float, scale:float, mask_path:str=DEFAULT_MASK) -> 'DigitalMap':
|
||||
dem = DigitalMap(seed, size, height, scale)
|
||||
dem.generate_elevation_map(mask_path)
|
||||
dem.generate_color_map()
|
||||
return dem
|
|
@ -1,36 +0,0 @@
|
|||
import numpy as np
|
||||
import cv2 as cv
|
||||
from multiprocessing import Pool, cpu_count
|
||||
|
||||
from ..terrain import Terrain
|
||||
|
||||
class Localizator:
|
||||
def __init__(self, terrain:Terrain, skyline:np.ndarray) -> None:
|
||||
self.terrain = terrain
|
||||
self.skyline = skyline
|
||||
|
||||
def _compute_score_mp(self, func:callable, args:any, print_progress:bool=True):
|
||||
if print_progress:
|
||||
import rich.progress as rp
|
||||
progress_bar = rp.Progress(
|
||||
*rp.Progress.get_default_columns(),
|
||||
rp.TimeElapsedColumn(),
|
||||
rp.MofNCompleteColumn()
|
||||
)
|
||||
task = progress_bar.add_task('Compute heatmap', total=len(args))
|
||||
progress_bar.start()
|
||||
|
||||
score_map = (self.terrain.dem.elevation != 0).astype(np.float16) * -1
|
||||
with Pool(processes=cpu_count()) as pool:
|
||||
results = pool.imap(func, args)
|
||||
for x, y, score in results:
|
||||
score_map[y, x] = np.clip(score, -65000, 65000)
|
||||
progress_bar.advance(task, 1)
|
||||
|
||||
if print_progress:
|
||||
progress_bar.stop()
|
||||
|
||||
return score_map
|
||||
|
||||
def generate_scoremap(self):
|
||||
raise NotImplementedError()
|
|
@ -1,35 +0,0 @@
|
|||
import numpy as np
|
||||
import math
|
||||
from scipy.fft import fft, ifft
|
||||
from scipy.signal import gaussian
|
||||
|
||||
from ..terrain import Terrain
|
||||
from .localizator import Localizator
|
||||
|
||||
def compute_score(args) -> float:
|
||||
x, y, F, H, G = args
|
||||
r = abs(ifft(H*G/F))
|
||||
shift = np.argmax(r)
|
||||
peak = r[shift]
|
||||
r[shift-5:shift+5] = 0
|
||||
score = peak/math.sqrt(np.sum(np.power(r,2)))
|
||||
return x, y, score
|
||||
|
||||
class MosseCorrelation(Localizator):
|
||||
def __init__(self, terrain: Terrain, skyline: np.ndarray) -> None:
|
||||
super().__init__(terrain, skyline)
|
||||
self.F = fft(skyline)
|
||||
self.G = fft(gaussian(skyline.shape[0], std=2.25,sym=True))
|
||||
|
||||
def generate_scoremap(self, ray_dist:float=1000.0):
|
||||
ray_count = self.F.shape[0]
|
||||
args = [(
|
||||
x,
|
||||
y,
|
||||
self.F,
|
||||
fft(self.terrain.compute_skyline(x,y,ray_count,ray_dist)),
|
||||
self.G
|
||||
) for x in range(self.terrain.dem.size) for y in range(self.terrain.dem.size) if self.terrain.dem.elevation[y,x] == 0]
|
||||
scoremap = self._compute_score_mp(compute_score, args)
|
||||
scoremap = (scoremap-np.min(scoremap)) / (np.max(scoremap)-np.min(scoremap))
|
||||
return scoremap
|
57
src/pyskyline/mosse_correlation.py
Normal file
57
src/pyskyline/mosse_correlation.py
Normal file
|
@ -0,0 +1,57 @@
|
|||
# This file is a application of 'GPS-level accurate camera localization with HorizonNet'
|
||||
# (DOI: 10.1002/rob.21929)
|
||||
|
||||
from scipy.signal.windows import gaussian, hamming
|
||||
from scipy.fft import fft, ifft
|
||||
from multiprocessing import Pool, cpu_count
|
||||
import numpy as np
|
||||
import math
|
||||
|
||||
from . import Terrain
|
||||
|
||||
DEFAULT_GAUSSIAN_STD = 2.25
|
||||
|
||||
def _compute_score(args) -> float:
|
||||
pos, F, H, G = args
|
||||
r = abs(ifft(H*G/F))
|
||||
shift = np.argmax(r)
|
||||
peak = r[shift]
|
||||
r[shift-5:shift+5] = 0
|
||||
score = peak/math.sqrt(np.sum(np.power(r,2)))
|
||||
return pos, score
|
||||
|
||||
class MosseCorrelation:
|
||||
@staticmethod
|
||||
def process(terrain:Terrain, skyline:np.ndarray, print_progress:bool=True):
|
||||
n = terrain._raycount
|
||||
window = hamming(n)
|
||||
G = fft(gaussian(n, DEFAULT_GAUSSIAN_STD))
|
||||
F = fft(skyline * window)
|
||||
score_map = np.zeros((terrain._size, terrain._size), dtype=float)
|
||||
args = [(
|
||||
(x, y),
|
||||
F,
|
||||
fft(terrain.get_skyline(x,y) * window),
|
||||
G
|
||||
) for x in range(terrain._size) for y in range(terrain._size) if terrain.is_water(x,y)]
|
||||
|
||||
if print_progress:
|
||||
import rich.progress as rp
|
||||
progress_bar = rp.Progress(
|
||||
*rp.Progress.get_default_columns(),
|
||||
rp.TimeElapsedColumn(),
|
||||
rp.MofNCompleteColumn()
|
||||
)
|
||||
task = progress_bar.add_task('Compute heatmap', total=len(args))
|
||||
progress_bar.start()
|
||||
|
||||
with Pool(processes=cpu_count()) as pool:
|
||||
results = pool.imap(_compute_score, args)
|
||||
for (x, y), score in results:
|
||||
score_map[y, x] = score
|
||||
progress_bar.advance(task, 1)
|
||||
|
||||
if print_progress:
|
||||
progress_bar.stop()
|
||||
|
||||
return score_map/np.max(score_map)
|
|
@ -1,113 +0,0 @@
|
|||
import numpy as np
|
||||
import cv2 as cv
|
||||
import math
|
||||
|
||||
from .digital_map import DigitalMap
|
||||
|
||||
def bresenham_line(x:int, y:int, dist:float, angle:float, limit:int) -> list:
|
||||
"""Generate list of points of all grid cell where raycast hit.
|
||||
see https://zingl.github.io/bresenham.html
|
||||
|
||||
Args:
|
||||
x (int): X coordinate.
|
||||
y (int): Y coordinate.
|
||||
dist (float): Ray distance.
|
||||
angle (float): Ray angle.
|
||||
limit (int): coordinate limit.
|
||||
|
||||
Returns:
|
||||
list: List of points [(x,y),...].
|
||||
"""
|
||||
points = []
|
||||
|
||||
x1 = int(x + np.cos(angle)* dist)
|
||||
y1 = int(y + np.sin(angle)* dist)
|
||||
dx = abs(x1 - x)
|
||||
sx = 1 if x < x1 else -1
|
||||
dy = -abs(y1 - y)
|
||||
sy = 1 if y < y1 else -1
|
||||
error = dx + dy
|
||||
|
||||
while (x != x1 or y != y1) and (0 <= x < limit and 0 <= y < limit):
|
||||
points.append((x,y))
|
||||
|
||||
e2 = 2 * error
|
||||
if e2 >= dy:
|
||||
error += dy
|
||||
x += sx
|
||||
if e2 <= dx:
|
||||
error += dx
|
||||
y += sy
|
||||
|
||||
return points
|
||||
|
||||
def get_highest_angle(points:list, dem:DigitalMap) -> tuple:
|
||||
m_height = 0
|
||||
m_angle = 0
|
||||
m_pos = points[-1]
|
||||
|
||||
for x, y in points[1:]:
|
||||
height = dem.elevation[y,x]
|
||||
if height > m_height:
|
||||
m_height = height
|
||||
dist = math.sqrt((points[0][0]-x)**2+(points[0][1]-y)**2) * dem.scale
|
||||
angle = np.arctan((height * dem.height/255)/dist)
|
||||
if angle >= m_angle:
|
||||
m_angle = angle
|
||||
m_pos = (x,y)
|
||||
|
||||
return np.rad2deg(m_angle), m_pos
|
||||
|
||||
def mp_compute_skyline(args:tuple) -> tuple:
|
||||
dem, x, y, ray_count, ray_dist = args
|
||||
return (x,y,compute_skyline(dem,x,y,ray_count,ray_dist))
|
||||
|
||||
def compute_skyline(dem:DigitalMap, x:int, y:int, ray_count:int, ray_dist:float) -> np.ndarray:
|
||||
"""_summary_
|
||||
|
||||
Args:
|
||||
x (int): X coordinate
|
||||
y (int): Y coordinate
|
||||
ray_count (int, optional): Raycast count. Defaults to 256.
|
||||
ray_dist (float, optional): Raycast distance in meter. Defaults to 1000.0.
|
||||
|
||||
Returns:
|
||||
np.ndarray: skyline array.
|
||||
"""
|
||||
ray_step = 2 * np.pi / (ray_count-1)
|
||||
line = np.zeros((ray_count), dtype=np.float16)
|
||||
|
||||
for i in range(ray_count):
|
||||
points = bresenham_line(x, y, ray_dist/dem.scale, ray_step*i, dem.size)
|
||||
angle, _ = get_highest_angle(points, dem)
|
||||
line[i] = angle
|
||||
|
||||
return line
|
||||
|
||||
def compute_fov(dem:DigitalMap, x:int, y:int, ray_count:int=256, ray_dist:float=1000.0) -> np.ndarray:
|
||||
"""_summary_
|
||||
|
||||
Args:
|
||||
x (int): X coordinate
|
||||
y (int): Y coordinate
|
||||
ray_count (int, optional): Raycast count. Defaults to 256.
|
||||
ray_dist (float, optional): Raycast distance in meter. Defaults to 1000.0.
|
||||
|
||||
Returns:
|
||||
np.ndarray: skyline array.
|
||||
"""
|
||||
ray_step = 2 * np.pi / (ray_count-1)
|
||||
output = np.copy(dem.color)
|
||||
|
||||
output = cv.circle(output, (x,y), 3, (255,125,0),-1)
|
||||
last_pos = None
|
||||
|
||||
for i in range(ray_count):
|
||||
points = bresenham_line(x, y, ray_dist/dem.scale, ray_step*i, dem.size)
|
||||
_, pos = get_highest_angle(points, dem)
|
||||
|
||||
if last_pos is not None:
|
||||
output = cv.line(output,last_pos,pos,(0,0,255),2)
|
||||
last_pos = pos
|
||||
|
||||
return output
|
183
src/pyskyline/terrain.py
Normal file → Executable file
183
src/pyskyline/terrain.py
Normal file → Executable file
|
@ -1,110 +1,99 @@
|
|||
from multiprocessing import Pool, cpu_count
|
||||
import numpy as np
|
||||
import cv2 as cv
|
||||
from .digital_map import DigitalMap
|
||||
from . import raycast
|
||||
import noise
|
||||
import ctypes
|
||||
import random
|
||||
from scipy import ndimage
|
||||
|
||||
class Terrain:
|
||||
def __init__(self, dem:DigitalMap) -> None:
|
||||
self.dem = dem
|
||||
self.skylines = {}
|
||||
from . import DEFAULT_RAYCOUNT, DEFAULT_RAYDIST
|
||||
|
||||
def compute_all_skylines(self, ray_count:int=256, ray_dist:float=1000.0, print_progress:bool=True):
|
||||
args = [(self.dem, x, y, ray_count, ray_dist) for x in range(self.dem.size) for y in range(self.dem.size) if self.dem.elevation[y,x] == 0]
|
||||
NOISE_OCTAVES : int = 6
|
||||
NOISE_PERSISTENCE : float = 0.5
|
||||
NOISE_LACUNARITY : float = 2.0
|
||||
COLOR_WATER : tuple = (169, 166, 97)
|
||||
COLOR_SAND : tuple = (175, 214, 238)
|
||||
COLOR_GRASS : tuple = ( 34, 139, 34)
|
||||
COLOR_TREE : tuple = ( 20, 100, 20)
|
||||
GROUND_LAYER : dict = {
|
||||
'ocean' : 135,
|
||||
'sand' : 150,
|
||||
'grass' : 170
|
||||
}
|
||||
|
||||
if print_progress:
|
||||
import rich.progress as rp
|
||||
progress_bar = rp.Progress(
|
||||
*rp.Progress.get_default_columns(),
|
||||
rp.TimeElapsedColumn(),
|
||||
rp.MofNCompleteColumn()
|
||||
)
|
||||
task = progress_bar.add_task('Compute skylines', total=len(args))
|
||||
progress_bar.start()
|
||||
_TerrainHandle = ctypes.POINTER(ctypes.c_char)
|
||||
_LIB = ctypes.cdll.LoadLibrary(f"{'/'.join(__file__.split('/')[:-2])}/cppskyline/lib/libconvert.so")
|
||||
_LIB.Terrain_new.argtypes = [ctypes.c_size_t, np.ctypeslib.ndpointer(dtype=np.uint8, ndim=2, flags='C_CONTIGUOUS'), ctypes.c_uint8, ctypes.c_float, ctypes.c_float, ctypes.c_uint16, ctypes.c_float ]
|
||||
_LIB.Terrain_new.restype = _TerrainHandle
|
||||
_LIB.Terrain_free.argtypes = [_TerrainHandle]
|
||||
_LIB.Terrain_free.restype = None
|
||||
_LIB.Terrain_get_skyline.argtypes = [_TerrainHandle, ctypes.c_uint32, ctypes.c_uint32]
|
||||
_LIB.Terrain_get_skyline.restype = ctypes.POINTER(ctypes.c_float)
|
||||
_LIB.Terrain_is_water.argtypes = [_TerrainHandle, ctypes.c_uint32, ctypes.c_uint32]
|
||||
_LIB.Terrain_is_water.restype = ctypes.c_bool
|
||||
_LIB.Terrain_precompute_all_skylines.argtypes = [_TerrainHandle]
|
||||
_LIB.Terrain_precompute_all_skylines.restype = None
|
||||
|
||||
with Pool(processes=cpu_count()) as pool:
|
||||
results = pool.imap(raycast.mp_compute_skyline, args)
|
||||
for x, y, skyline in results:
|
||||
self.skylines[(x,y)] = skyline
|
||||
progress_bar.advance(task, 1)
|
||||
class Terrain():
|
||||
def __init__(self, seed:int, size:int, scale:float, dem:np.ndarray, max_altitude:float, raycount:int, raydist:float) -> None:
|
||||
self._seed = seed
|
||||
self._size = size
|
||||
self._scale = scale
|
||||
self._dem = dem
|
||||
self._max_altitude = max_altitude
|
||||
self._raycount = raycount
|
||||
self.obj = _LIB.Terrain_new(size, dem, GROUND_LAYER['ocean'], max_altitude, scale, raycount, raydist)
|
||||
|
||||
if print_progress:
|
||||
progress_bar.stop()
|
||||
|
||||
def compute_skyline(self, x:int, y:int, ray_count:int=256, ray_dist:float=1000.0):
|
||||
"""_summary_
|
||||
|
||||
Args:
|
||||
x (int): X coordinate
|
||||
y (int): Y coordinate
|
||||
ray_count (int, optional): Raycast count. Defaults to 256.
|
||||
ray_dist (float, optional): Raycast distance in meter. Defaults to 1000.0.
|
||||
|
||||
Returns:
|
||||
np.ndarray: skyline array.
|
||||
"""
|
||||
if (x,y) not in self.skylines:
|
||||
self.skylines[(x,y)] = raycast.compute_skyline(self.dem, x, y, ray_count, ray_dist)
|
||||
return self.skylines[(x,y)]
|
||||
|
||||
def compute_fov(self, x:int, y:int, ray_count:int=256, ray_dist:float=1000.0):
|
||||
"""_summary_
|
||||
|
||||
Args:
|
||||
x (int): X coordinate
|
||||
y (int): Y coordinate
|
||||
ray_count (int, optional): Raycast count. Defaults to 256.
|
||||
ray_dist (float, optional): Raycast distance in meter. Defaults to 1000.0.
|
||||
|
||||
Returns:
|
||||
np.ndarray: skyline array.
|
||||
"""
|
||||
return raycast.compute_fov(self.dem, x, y, ray_count, ray_dist)
|
||||
|
||||
def save(self, file:str):
|
||||
np.savez_compressed(f'{file}.npz',
|
||||
size = self.dem.size,
|
||||
seed = self.dem.seed,
|
||||
height = self.dem.height,
|
||||
scale = self.dem.scale,
|
||||
elevation_map = self.dem.elevation,
|
||||
skylines = self.skylines
|
||||
)
|
||||
def __del__(self):
|
||||
_LIB.Terrain_free(self.obj)
|
||||
|
||||
@staticmethod
|
||||
def load(file:str) -> 'Terrain':
|
||||
if not file.endswith('.npz'):
|
||||
file += ".npz"
|
||||
save = np.load(file,allow_pickle=True)
|
||||
dem = DigitalMap(
|
||||
seed = save['seed'],
|
||||
size = save['size'],
|
||||
height = save['height'],
|
||||
scale = save['scale']
|
||||
)
|
||||
dem.elevation = save.get('elevation_map')
|
||||
dem.generate_color_map()
|
||||
terrain = Terrain(dem)
|
||||
terrain.skylines = save['skylines'].item()
|
||||
def generate(seed:int, size:int, scale:float, max_altitude:float, raycount:int=DEFAULT_RAYCOUNT, raydist:float=DEFAULT_RAYDIST) -> 'Terrain':
|
||||
dem = np.ndarray((size,size),np.uint8)
|
||||
|
||||
return terrain
|
||||
for x in range(size):
|
||||
for y in range(size):
|
||||
dem[y,x] = (noise.snoise2(
|
||||
x * scale / 1000 ,
|
||||
y * scale / 1000 ,
|
||||
NOISE_OCTAVES,
|
||||
NOISE_PERSISTENCE,
|
||||
NOISE_LACUNARITY,
|
||||
base=seed
|
||||
) + 1) * 127.5
|
||||
|
||||
@staticmethod
|
||||
def generate(seed:int=0x7B16, size:int=512, height:float=25.0, scale:float=1.0,
|
||||
mask_path:str=DigitalMap.DEFAULT_MASK) -> 'Terrain':
|
||||
""" Generate a procedural terrain using seed and mask, and precompute all horizon lines
|
||||
return Terrain(seed, size, scale, dem, max_altitude, raycount, raydist)
|
||||
|
||||
Args:
|
||||
seed (int, optional): Random seed for the terrain generation. Defaults to 0x7B16.
|
||||
size (int, optional): Size of the generated elevation map in pixel. Defaults to 512.
|
||||
height (float, optional): Maximum altitude of the elevation map. Defaults to 40.0.
|
||||
scale (float, optional): Scale of a pixel in meter. Defaults to 1.0.
|
||||
mask_path (str, optional): Terrain mask path. Defaults to DEFAULT_MASK.
|
||||
def get_skyline(self, x:int, y:int) -> np.ndarray:
|
||||
skyline = _LIB.Terrain_get_skyline(self.obj, x, y)
|
||||
return np.ctypeslib.as_array(skyline, shape=(self._raycount,))
|
||||
|
||||
Returns:
|
||||
Terrain: _description_
|
||||
"""
|
||||
dem = DigitalMap.generate(seed, size, height, scale, mask_path)
|
||||
terrain = Terrain(dem)
|
||||
def is_water(self, x:int, y:int) -> bool:
|
||||
return _LIB.Terrain_is_water(self.obj, x, y)
|
||||
|
||||
return terrain
|
||||
def precompute_all_skylines(self) -> None:
|
||||
import time
|
||||
a = time.time()
|
||||
_LIB.Terrain_precompute_all_skylines(self.obj)
|
||||
print(f"{(time.time()-a):.1f}s")
|
||||
|
||||
def get_random_water_coordinate(self, coastal_range=100.0) -> tuple:
|
||||
r = int(round(coastal_range/self._scale))
|
||||
valid_location = self._dem <= GROUND_LAYER['ocean']
|
||||
valid_location = ndimage.binary_erosion(valid_location, structure=np.ones((r,r)))
|
||||
while True:
|
||||
x = random.randint(0, self._size)
|
||||
y = random.randint(0, self._size)
|
||||
if valid_location[y,x] == 1:
|
||||
return (x,y)
|
||||
|
||||
def rgb(self) -> np.ndarray:
|
||||
rgb = np.full((self._size, self._size, 3), COLOR_WATER, dtype=np.uint8)
|
||||
rgb[self._dem > GROUND_LAYER['ocean']] = COLOR_SAND
|
||||
rgb[self._dem > GROUND_LAYER['sand']] = COLOR_GRASS
|
||||
rgb[self._dem > GROUND_LAYER['grass']] = COLOR_TREE
|
||||
return np.asarray(rgb)
|
||||
|
||||
def grayscale(self) -> np.ndarray:
|
||||
return self._dem
|
||||
|
||||
def water_tile_count(self) -> int:
|
||||
return (self._dem <= GROUND_LAYER['ocean']).sum()
|
||||
|
|
Loading…
Reference in a new issue