working cpp and mosse
This commit is contained in:
commit
bc69ad483b
27 changed files with 867 additions and 0 deletions
8
.gitignore
vendored
Executable file
8
.gitignore
vendored
Executable file
|
@ -0,0 +1,8 @@
|
|||
# Python
|
||||
**/__pycache__
|
||||
**/*.egg-info
|
||||
|
||||
# Example
|
||||
**/example/*.jpg
|
||||
**/example/*.png
|
||||
**/example/*.svg
|
0
README.md
Executable file
0
README.md
Executable file
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
|
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()
|
134
example/mosse_viz.py
Executable file
134
example/mosse_viz.py
Executable file
|
@ -0,0 +1,134 @@
|
|||
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()
|
21
pyproject.toml
Executable file
21
pyproject.toml
Executable file
|
@ -0,0 +1,21 @@
|
|||
[build-system]
|
||||
requires = ["setuptools>=61.0"]
|
||||
build-backend = "setuptools.build_meta"
|
||||
[project]
|
||||
name = "pyskyline"
|
||||
version = "0.1"
|
||||
authors = [
|
||||
{ name="Alexandre FOUCHER", email="alexandre.foucher@univ-ubs.fr" },
|
||||
]
|
||||
description = "A small example package"
|
||||
readme = "README.md"
|
||||
requires-python = ">=3.8"
|
||||
classifiers = [
|
||||
"Programming Language :: Python :: 3",
|
||||
"License :: OSI Approved :: MIT License",
|
||||
"Operating System :: OS Independent",
|
||||
]
|
||||
|
||||
# [project.urls]
|
||||
# Homepage = "https://github.com/pypa/sampleproject"
|
||||
# Issues = "https://github.com/pypa/sampleproject/issues"
|
2
requirements.txt
Normal file
2
requirements.txt
Normal file
|
@ -0,0 +1,2 @@
|
|||
noise==1.2.2
|
||||
scipy==1.14.1
|
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::uint64_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::int32_t x, std::int32_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;
|
||||
};
|
128
src/cppskyline/src/Terrain.cpp
Normal file
128
src/cppskyline/src/Terrain.cpp
Normal file
|
@ -0,0 +1,128 @@
|
|||
# include "Terrain.hpp"
|
||||
# include <math.h>
|
||||
# include <iostream>
|
||||
|
||||
// ============================================================================
|
||||
// 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::int32_t x, std::int32_t y)
|
||||
{
|
||||
std::uint64_t id = y * this->m_size + 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()
|
||||
{
|
||||
#pragma omp parallel for collapse(2)
|
||||
for( std::uint32_t x=0; x<this->m_size; x++)
|
||||
{
|
||||
for( std::uint32_t y=0; y<this->m_size; y++)
|
||||
{
|
||||
if (this->is_water(x,y))
|
||||
{
|
||||
std::uint64_t id = y * this->m_size + x;
|
||||
if (this->m_skylines.find(id) == this->m_skylines.end()) {
|
||||
this->m_skylines.insert({id, this->compute_skyline(x, y)});
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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); }
|
||||
};
|
5
src/pyskyline/__init__.py
Executable file
5
src/pyskyline/__init__.py
Executable file
|
@ -0,0 +1,5 @@
|
|||
DEFAULT_RAYCOUNT = 256
|
||||
DEFAULT_RAYDIST = 1000.0
|
||||
|
||||
from .terrain import Terrain
|
||||
from .mosse_correlation import MosseCorrelation
|
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
|
84
src/pyskyline/terrain.py
Executable file
84
src/pyskyline/terrain.py
Executable file
|
@ -0,0 +1,84 @@
|
|||
import numpy as np
|
||||
import noise
|
||||
import ctypes
|
||||
|
||||
from . import DEFAULT_RAYCOUNT, DEFAULT_RAYDIST
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
_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
|
||||
|
||||
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)
|
||||
|
||||
def __del__(self):
|
||||
_LIB.Terrain_free(self.obj)
|
||||
|
||||
@staticmethod
|
||||
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)
|
||||
|
||||
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
|
||||
|
||||
return Terrain(seed, size, scale, dem, max_altitude, raycount, raydist)
|
||||
|
||||
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,))
|
||||
|
||||
def is_water(self, x:int, y:int) -> bool:
|
||||
return _LIB.Terrain_is_water(self.obj, x, y)
|
||||
|
||||
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 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 rgb
|
||||
|
||||
def grayscale(self) -> np.ndarray:
|
||||
return self._dem
|
Loading…
Reference in a new issue