Initial commit

.gitignore

+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+CMakeLists.txt.user
+CMakeCache.txt
+CMakeFiles
+CMakeScripts
+Testing
+Makefile
+cmake_install.cmake
+install_manifest.txt
+compile_commands.json
+CTestTestfile.cmake
+_deps
+
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/
+
+.vscode/
\ No newline at end of file

CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.12)
+
+project(sandbox_wrapper)
+
+find_package(OpenCV REQUIRED)
+include_directories(${OpenCV_INCLUDE_DIRS})
+include_directories(/usr/include/x86_64-linux-gnu/python3.6m)
+include_directories(/usr/include/python3.6m)
+include_directories(/src/ar_sandbox_lib/inc)
+
+
+include_directories(/usr/local/lib/python3.6/dist-packages/pybind11/share/cmake/pybind11)
+find_package(pybind11 REQUIRED)
+pybind11_add_module(sandbox_wrapper ./cpp/sandbox_wrapper.cpp)
+
+include_directories(/src/ar_sandbox_lib/build/)
+target_link_libraries(sandbox_wrapper PRIVATE ${OpenCV_LIBS})
+target_link_libraries(sandbox_wrapper PRIVATE /src/ar_sandbox_lib/build/libsandbox.so.1.0.0 -lrealsense2 -lyaml-cpp)

README.md

+# AR_Sandbox
+
+Simple python wrapper for ar_sandbox_lib
+
+
+## How to use
+
+```python
+from ar_sandbox.sandbox import Sandbox
+
+def my_function(depth_matrix, frame):
+    # Do something with the depth
+    # This function should return a 3 channels RGB matrix
+    # of the same shape as frame.
+    return result
+
+box = Sandbox(refresh=250)
+box.init("config.yaml")
+box.func = my_function
+box.start()
+```
\ No newline at end of file

cpp/sandbox_wrapper.cpp

+#include <opencv2/core/core.hpp>
+#include <pybind11/pybind11.h>
+#include <pybind11/numpy.h>
+#include <pybind11/stl.h>
+#include "../../ar_sandbox_lib/inc/sandbox.h"
+
+
+// Convert a type to a readable opencv Type format.
+// https://stackoverflow.com/questions/10167534/how-to-find-out-what-type-of-a-mat-object-is-with-mattype-in-opencv
+std::string cvtype2str(int type) {
+    std::string r;
+    unsigned char depth = type & CV_MAT_DEPTH_MASK;
+    unsigned char chans = 1 + (type >> CV_CN_SHIFT);
+    switch ( depth ) {
+        case CV_8U:  r = "CV_8U"; break;
+        case CV_8S:  r = "CV_8S"; break;
+        case CV_16U: r = "CV_16U"; break;
+        case CV_16S: r = "CV_16S"; break;
+        case CV_32S: r = "CV_32S"; break;
+        case CV_32F: r = "CV_32F"; break;
+        case CV_64F: r = "CV_64F"; break;
+        default:     r = "CV_User"; break;
+    }
+    r += "C";
+    r += (chans+'0');
+    return r;
+}
+
+// Print some info on the provided matrix.
+void printMatrixInfo(cv::Mat &matrix)
+{
+    std::cout << "MATRIX :" << std::endl;
+    std::cout << matrix << std::endl;
+    std::cout << "Channels: ";
+    std::cout << matrix.channels() << std::endl;
+    std::cout << "Rows: ";
+    std::cout << matrix.rows << std::endl;
+    std::cout << "Cols: ";
+    std::cout << matrix.cols << std::endl;
+    std::cout << "Depth: ";
+    std::cout << cvtype2str(matrix.type()) << std::endl;
+}
+
+// Remove this
+int sz_cols = 5;
+int sz_rows = 6;
+
+// Remove this
+void populate3Channels(cv::Mat &mat) {
+    if (mat.channels() == 3) {
+        for (int c = 0; c < mat.channels(); c++) {
+            for (int i = 0; i < mat.rows; i++) {
+                for (int j = 0;j < mat.cols; j++) {
+                    mat.at<cv::Vec3b>(i, j) = cv::Vec3b(i * mat.cols + j, i * mat.cols + j, i * mat.cols + j);
+                }
+            }
+        }
+    }
+}
+
+// Remove this
+void populate1Channel(cv::Mat &mat) {
+    for (int i = 0; i < mat.rows; i++) {
+        for (int j = 0;j < mat.cols; j++) {
+            mat.at<float>(i, j) = i * mat.cols + j;
+        }
+    }
+}
+// Give dummy matrixes. Should be deleted.
+cv::Mat_<cv::Vec3b> giveMatrix() {
+    cv::Mat_<cv::Vec3b> matrix = cv::Mat_<cv::Vec3b>::zeros(sz_cols, sz_rows);
+    populate3Channels(matrix);
+    // printMatrixInfo(matrix);
+    cv::Rect rec(1, 1, 3, 3);
+    cv::Mat crop = matrix(rec);
+    cv::Mat outmat;
+    crop.copyTo(outmat);
+    // printMatrixInfo(outmat);
+    return outmat;
+}
+
+cv::Mat_<float> giveFloatMatrix() {
+    cv::Mat_<float> matrix = cv::Mat_<float>::zeros(sz_cols, sz_rows);
+    populate1Channel(matrix);
+    // printMatrixInfo(matrix);
+    cv::Rect rec(1, 1, 3, 3);
+    cv::Mat crop = matrix(rec);
+    cv::Mat outmat;
+    crop.copyTo(outmat);
+    // printMatrixInfo(outmat);
+    return outmat;
+}
+
+// Dummy function to check if parameters are valid, for testing purposes.
+// This should obviously be deleted.
+int adjProj(Sandbox &self, cv::Mat_<cv::Vec3b> frame, cv::Mat_<float> depth) {
+    // printMatrixInfo(frame);
+    // printMatrixInfo(depth);
+    return 0;
+}
+
+
+// Create a cv::Mat<Vec3b> from a numpy array.
+// Used as a constructor for cvMat3b
+cv::Mat_<cv::Vec3b> npBufferToCvMat3b(pybind11::array_t<unsigned char>& arr) {
+    pybind11::buffer_info buf = arr.request();
+    cv::Mat_<cv::Vec3b> m;
+    cv::Mat mat(buf.shape[0], buf.shape[1], CV_8UC3, (unsigned char*)buf.ptr);
+    m = mat;
+    return m;
+}
+
+// Create a cv::Mat<float> from a numpy array.
+// Used as a constructor for cvMatf
+cv::Mat_<float> npBufferToCvMatF(pybind11::array_t<float>& arr) {
+    pybind11::buffer_info buf = arr.request();
+    cv::Mat_<float> m;
+    cv::Mat mat(buf.shape[0], buf.shape[1], CV_32F, (unsigned char*)buf.ptr);
+    // return self;
+    m = mat;
+    return m;
+}
+
+// Create a numpy-readable buffer from a cv::Mat_<float>.
+pybind11::buffer_info cvMatFToNpBuffer(cv::Mat_<float>& im) {
+    return pybind11::buffer_info(
+        // Pointer to buffer
+        im.data,
+        // Size of one element
+        sizeof(float),
+        // Python struct-style format descriptor
+        pybind11::format_descriptor<float>::format(),
+        // Number of dimensions
+        2,
+        // Buffer dimensions
+        { im.rows, im.cols },
+        // Strides (in bytes) for each index
+        {
+            sizeof(float) * im.cols,
+            sizeof(float)
+        });
+}
+
+// Create a numpy-readable buffer from a cv::Mat_<Vec3b>.
+pybind11::buffer_info cvMat3bToNpBuffer(cv::Mat_<cv::Vec3b>& im) {
+    return pybind11::buffer_info(
+        // Pointer to buffer
+        im.data,
+        // Size of one element
+        sizeof(unsigned char),
+        // Python struct-style format descriptor
+        pybind11::format_descriptor<unsigned char>::format(),
+        // Number of dimensions
+        3,
+        // Buffer dimensions
+        { im.rows, im.cols, im.channels() },
+        // Strides (in bytes) for each index
+        {
+            sizeof(unsigned char) * im.channels() * im.cols,
+            sizeof(unsigned char) * im.channels(),
+            sizeof(unsigned char)
+        });
+}
+
+
+PYBIND11_MODULE(sandbox_wrapper, m) {
+    m.def("new", giveMatrix);
+    m.def("newf", giveFloatMatrix);
+    m.def("print_matrix_info",[](pybind11::object mat, int type){
+        cv::Mat m;
+        switch (type) {
+            case CV_8UC3:
+                m = mat.cast<cv::Mat_<cv::Vec3b>>();
+                break;
+            case CV_32FC1:
+                m = mat.cast<cv::Mat_<float>>();
+                break;
+            default:
+                break;
+        }
+        return printMatrixInfo(m);
+    });
+
+    // cvMat classes
+    pybind11::class_<cv::Mat_<cv::Vec3b>>(m, "cvMat3b", pybind11::buffer_protocol())
+        .def(pybind11::init(&npBufferToCvMat3b))
+        .def_buffer(&cvMat3bToNpBuffer);
+    pybind11::class_<cv::Mat_<float>>(m, "cvMatf", pybind11::buffer_protocol())
+        .def(pybind11::init(&npBufferToCvMatF))
+        .def_buffer(&cvMatFToNpBuffer);
+
+    // Sandbox pybind11 wrapper
+    pybind11::class_<Sandbox>(m, "Sandbox")
+        .def(pybind11::init<>())
+        .def("init_sandbox",  [](Sandbox &self){
+            return self.init();
+        })
+        .def("load_config", [](Sandbox &self, char* path){
+            return self.loadConfig(path);
+        })
+        .def("capture_frame", [](Sandbox &self){
+            return self.captureFrame();
+        })
+        .def("adjust_projection", [](Sandbox &self, cv::Mat_<cv::Vec3b> frame, cv::Mat_<float> depth) {
+            return self.adjustProjection(frame, depth);
+        })
+        .def("get_color_frame", [](Sandbox &self){
+            cv::Mat_<cv::Vec3b> out;
+            cv::Mat_<cv::Vec3b> cropped = self.getColorFrame();
+            cropped.copyTo(out);
+            return out;
+            // return giveMatrix();
+        })
+        .def("get_depth_frame", [](Sandbox &self){
+            cv::Mat_<float> out;
+            cv::Mat_<float> cropped = self.getDepthFrame();
+            cropped.copyTo(out);
+            return out;
+            // return giveFloatMatrix();
+        })
+        .def("get_distance_top", [](Sandbox &self){
+            return self.getProjection()->getDistanceTopSandbox();
+        })
+        ;
+}
\ No newline at end of file

pyproject.toml

+[build-system]
+requires = [
+    "setuptools>=42",
+    "wheel"
+]
+
+build-backend = "setuptools.build_meta"
\ No newline at end of file

setup.cfg

+[metadata]
+name = ar_sandbox
+version = 0.0.1
+author = Alexis Durgnat
+author_email = alexis.durgnat@hesge.ch
+description = AR Sandbox wrapper
+long_description = file: README.md
+long_description_content_type = text/markdown
+url = https://gitedu.hesge.ch/ar_sandbox
+
+[options]
+package_dir =
+    = src
+packages = find:
+python_requires = >=3.6
+
+
+[options.packages.find]
+where = src
+
+[options.package_data]
+* = sandbox_wrapper*.so
\ No newline at end of file

src/ar_sandbox/__init__.py

+from .sandbox import Sandbox, DummySandbox
\ No newline at end of file

src/ar_sandbox/examples/__init__.py

+from .examples import LevelDisplay
\ No newline at end of file

src/ar_sandbox/examples/examples.py

+import numpy as np
+
+COLORS = [
+    (0, [255, 0, 0]),
+    (0.25, [255, 255, 0]),
+    (0.5, [0, 255, 0]),
+    (0.75, [0, 255, 255]),
+    (1, [0, 0, 255])
+]
+
+class LevelDisplay():
+    colormap = COLORS
+    
+    @staticmethod
+    def get_color(depth_matrix, frame, draw_lines=True, colormap=COLORS):
+        points = np.array([c[0] for c in colormap])
+        r = [c[1][2] for c in colormap]
+        g = [c[1][1] for c in colormap]
+        b = [c[1][0] for c in colormap]
+        
+        # print(rval)
+        if draw_lines: line_mask = LevelDisplay.draw_lines(depth_matrix, points)
+        # print(line_mask.shape)
+        rval = np.interp(depth_matrix, points, r)
+        gval = np.interp(depth_matrix, points, g)
+        bval = np.interp(depth_matrix, points, b)
+        res = np.dstack((bval, gval, rval)).astype(np.uint8)
+        if draw_lines: res[line_mask,:] = 0 
+        return res
+
+    @staticmethod
+    def draw_lines(depth, points, width=0.025, between_levels=False):
+        lines = []
+        mask = np.ma.make_mask(np.zeros(depth.shape), shrink=False)
+        if between_levels:
+            ## TRANSITION BETWEEN
+            for (i,),p in np.ndenumerate(points[:-1]):
+                next = points[i + 1]
+                lines.append(((next-p)/2) + p)
+        else:
+            ## TRANSITION ON TOP
+            for (i,),p in np.ndenumerate(points[:-1]):
+                lines.append(p)
+        halfwidth = width/2
+        for line in lines:
+            mask = np.where(np.logical_or(mask, np.logical_and(depth >= line - halfwidth, depth <= line+halfwidth)), True, False)
+            # rpos_mline = np.interp([rpos_mline])
+        # print(mask)
+        return mask
\ No newline at end of file

src/ar_sandbox/sandbox.py

+import numpy as np
+import cv2
+import time
+from .wrapper import sandbox_wrapper as sw
+# import sandbox_wrapper as sw
+
+WINDOW_NAME='Remote'
+ENABLE_WINDOW = True
+
+class Sandbox():
+    class SandboxException(Exception):
+        ...
+
+    _cppbox=None
+    _verbosity = 0
+    # running = False
+    # refresh_rate = 500 #ms
+    # on_frame = None
+    # initialized = False
+
+    # display=ENABLE_WINDOW
+
+    def __init__(self, refresh=100, display_enabled=ENABLE_WINDOW) -> None:
+        """
+        Instantiate the sandbox.
+
+        Arguments:
+            refresh : The refresh rate in miliseconds
+            display_enabled : If set to false, won't display a window
+        """
+        self._cppbox = sw.Sandbox()
+        self.refresh_rate = refresh
+        self.display = display_enabled
+        self.running = False
+        self.on_frame = None
+        self.initialized = False
+
+
+    def init(self, config_file=None) -> bool:
+        """
+        Arguments:
+            config_file : A path to a sandbox configuration file. If omitted,
+            look for a file named "sandbox_conf.yaml" in the current directory.
+        """
+        if config_file is None:
+            config_file = "./sandbox_conf.yaml"
+        loaded = self._cppbox.load_config(config_file)
+        inited = self._cppbox.init_sandbox()
+        self.initialized = not loaded and not inited
+        return self.initialized
+    
+    def start(self, *args, **kwargs) -> None:
+        """
+        Start the sandbox.
+        
+        Arguments:
+            All arguments are passed down to the potential "on_frame"
+            callback function       
+        """
+        self._main_loop(args, kwargs)
+
+    def stop(self) -> None:
+        """
+        Stop the sandbox.
+        """
+        self.running = False
+
+    def capture(self) -> None:
+        """
+        Order a capture from the camera
+        """
+        self._cppbox.capture_frame()
+        # return True
+    
+    def get_depth_frame(self) -> np.ndarray:
+        """
+        Get the depth frame of the sandbox
+
+        Return:
+            A 1-channel numpy array of floats
+        """
+        return np.array(self._cppbox.get_depth_frame(), copy=False)
+
+    def get_color_frame(self) -> np.ndarray:
+        """
+        Get the color frame of the sandbox
+
+        Return:
+            A 3-channel numpy array of unsigned bytes
+        """
+        return np.array(self._cppbox.get_color_frame(), copy=False)
+
+    def adjust_projection(self, frame, depth) -> np.ndarray:
+        """
+        Adjust the frame to be correctly projected from the beamer.
+
+        Return:
+            the adjusted frame
+        """
+        frame = self._cppbox.adjust_projection(sw.cvMat3b(frame), sw.cvMatf(depth))
+        return np.array(frame)
+
+    def get_distance_top(self) -> float:
+        """
+        Get the distance from the top of the sandbox.
+        
+        Return:
+            A float (Distance from top, according to calibration)
+        """
+        return self._cppbox.get_distance_top()
+
+    def _main_loop(self, func_args=(), func_kwargs={}) -> None:
+        """
+        Main loop. Shouldn't be used directly, call start() instead.
+
+        When called, begin to run the sandbox.
+        Perform a capture, get the frames, call a function on it (on_frame),
+        adjust the projection and display it using opencv.
+
+        Arguments:
+            func_args : A n-tuple of args. Passed to the callback
+            func_kwargs : A dictionnary of named arguments. Passed to the callback
+        """
+        if not self.initialized:
+            raise Sandbox.SandboxException("Not initialized. Cannot run.")
+        self.running = True
+        # Create window
+        if self.display:
+            cv2.namedWindow(WINDOW_NAME, cv2.WND_PROP_FULLSCREEN)
+            cv2.setWindowProperty(WINDOW_NAME, cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)
+        now = time.time() # For debug purposes.
+        # Begin main loop.
+        while(self.running):
+            # Order a capture
+            self.capture()
+            # Take the current frame (Mandatory ?)
+            curr_frame = self.get_color_frame()
+            # Take the depth frame of the camera
+            curr_depth = self.get_depth_frame()
+
+            # If we have a function binded here, execute it.
+            if self.on_frame:
+                # Override the current frame
+                curr_frame = self.on_frame(curr_depth, curr_frame, *func_args, **func_kwargs)
+
+            # Adjust the projection according to the sandbox parameter
+            curr_frame = self.adjust_projection(curr_frame, curr_depth)
+
+            if self.display:
+                # Show the result
+                cv2.imshow(winname=WINDOW_NAME, mat=curr_frame)
+                if cv2.getWindowProperty(WINDOW_NAME,cv2.WND_PROP_VISIBLE) < 1:        
+                    break
+                if cv2.waitKey(self.refresh_rate) == 27:
+                    break
+            else:
+                time.sleep(self.refresh_rate * 0.001)
+            # Compute loop time, if we are verbose.        
+            if self._verbosity > 2:
+                ti = time.time()
+                t_diff = ti - now
+                now = ti
+                print(f"\tLoop time: {t_diff:.4f} sec\t", end="\r")
+            
+        # End the loop, destroy everything, output a new line to avoid loosing the last output.
+        if self._verbosity > 2: print("\n") 
+        if self.display:
+            cv2.destroyAllWindows()
+        # Set running to false
+        self.running = False
+
+
+class DummySandbox(Sandbox):
+    def __init__(self, im, refresh=100, display_enabled=False, noise=True) -> None:
+        self.noise = noise
+        super().__init__(refresh, display_enabled)
+        if type(im) == np.ndarray:
+            if im.ndim == 1:
+                self.im = im
+                self.imc = np.dstack((im, im, im))
+            elif im.ndim == 3:
+                self.imc = im
+                self.im = self.imc[:,:,0]
+            else:
+                raise Sandbox.SandboxException("Failure.")
+        else:
+            self.im = cv2.imread(im, flags=cv2.IMREAD_GRAYSCALE)
+            self.imc = cv2.imread(im, flags=cv2.IMREAD_COLOR)
+        self.factor = 1.0/255.0
+    
+    def init(self, config_file=None):
+        if config_file is None:
+            config_file = "./sandbox_conf.yaml"
+        loaded = self._cppbox.load_config(config_file)
+        inited = self._cppbox.init_sandbox()
+        inited = 0 # We don't care if we can't connect to the devices
+        # inverted logic, 0 is good. We don't care about inited
+        self.initialized = not loaded and not inited
+        return self.initialized
+    
+
+    def capture(self):
+        pass
+
+    def get_depth_frame(self):
+        n = np.array(self.im).astype(np.float32)
+        n *= self.factor
+        return n
+
+    def get_color_frame(self):
+        if self.noise:
+            mat = np.random.rand(self.imc.shape[0], self.imc.shape[1], self.imc.shape[2])
+            mat *= 255
+            return mat.astype(np.uint8)
+        else: return self.imc
+
+    def adjust_projection(self, frame, depth):
+        return frame
\ No newline at end of file