From 14db8a7983b85982e93ddcb6f77fa3d4dc49ccd0 Mon Sep 17 00:00:00 2001 From: umgnunes Date: Thu, 12 Oct 2023 09:55:21 +0200 Subject: [PATCH] Initial commit --- .clang-format | 7 + .dockerignore | 40 + .gitignore | 38 + CMakeLists.txt | 68 ++ Dockerfile | 40 + LICENSE | 360 +++++++++ README.md | 134 ++++ doc/CMakeLists.txt | 28 + doc/Doxyfile.in | 11 + doc/main.dox | 4 + include/ETTCM.hpp | 19 + include/ETTCM/assert.hpp | 31 + include/ETTCM/global_decay.hpp | 217 ++++++ include/ETTCM/global_motion_inverse_depth.hpp | 686 ++++++++++++++++++ include/ETTCM/leaky_sampling.hpp | 271 +++++++ include/ETTCM/motion_model.hpp | 508 +++++++++++++ include/ETTCM/rectify.hpp | 234 ++++++ include/ETTCM/types.hpp | 326 +++++++++ include/ETTCM/utils.hpp | 274 +++++++ src/CMakeLists.txt | 31 + src/global_divergence.hpp | 199 +++++ src/global_divergence_scaling.cpp | 3 + src/global_divergence_six_dof.cpp | 3 + src/global_divergence_translation.cpp | 3 + table_2_partial_results.sh | 28 + table_2_results.sh | 28 + table_6_partial_results.sh | 33 + table_6_results.sh | 33 + 28 files changed, 3657 insertions(+) create mode 100644 .clang-format create mode 100644 .dockerignore create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 Dockerfile create mode 100755 LICENSE create mode 100644 README.md create mode 100644 doc/CMakeLists.txt create mode 100644 doc/Doxyfile.in create mode 100644 doc/main.dox create mode 100644 include/ETTCM.hpp create mode 100644 include/ETTCM/assert.hpp create mode 100644 include/ETTCM/global_decay.hpp create mode 100644 include/ETTCM/global_motion_inverse_depth.hpp create mode 100644 include/ETTCM/leaky_sampling.hpp create mode 100644 include/ETTCM/motion_model.hpp create mode 100644 include/ETTCM/rectify.hpp create mode 100644 include/ETTCM/types.hpp create mode 100644 include/ETTCM/utils.hpp create mode 100644 src/CMakeLists.txt create mode 100644 src/global_divergence.hpp create mode 100644 src/global_divergence_scaling.cpp create mode 100644 src/global_divergence_six_dof.cpp create mode 100644 src/global_divergence_translation.cpp create mode 100644 table_2_partial_results.sh create mode 100644 table_2_results.sh create mode 100644 table_6_partial_results.sh create mode 100644 table_6_results.sh diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..7170de9 --- /dev/null +++ b/.clang-format @@ -0,0 +1,7 @@ +--- +BasedOnStyle: Google +AlwaysBreakAfterDefinitionReturnType: All +AlwaysBreakAfterReturnType: All +BreakBeforeBraces: Allman +DerivePointerAlignment: 'false' +... diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 0000000..128a3be --- /dev/null +++ b/.dockerignore @@ -0,0 +1,40 @@ +# 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 + +# Created folders +build +*~ + +# Dockerfile +Dockerfile +.dockerignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..fe70165 --- /dev/null +++ b/.gitignore @@ -0,0 +1,38 @@ +# 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 + +# Created folders +build +data +datasets +*~ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..7973731 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.16.3) + +set(LIB_NAME ETTCM) +project(${LIB_NAME} LANGUAGES CXX) + +# C++ version +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +set(${LIB_NAME}_DOC_DIR ${CMAKE_CURRENT_SOURCE_DIR}/doc) +set(${LIB_NAME}_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/include) +set(${LIB_NAME}_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src) + +# CXX flags +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic -Werror -march=native -mtune=native -fPIC") + +# Default to Release +set(DEFAULT_BUILD_TYPE "Release") +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + message(STATUS "Setting build type to '${DEFAULT_BUILD_TYPE}' as none was specified.") + set(CMAKE_BUILD_TYPE "${DEFAULT_BUILD_TYPE}" CACHE STRING "Choose the type of build." FORCE) + # Set the possible values of build type for cmake-gui + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS + "Debug" "Release" "MinSizeRel" "RelWithDebInfo") +endif() + +# Options +option(${LIB_NAME}_BUILD_DOC "Build documentation" ON) + +# Dependencies +include(FetchContent) +set(FETCHCONTENT_QUIET OFF) + +# pontella +FetchContent_Declare( + pontella + GIT_REPOSITORY https://github.com/neuromorphic-paris/pontella + GIT_TAG master +) +FetchContent_MakeAvailable(pontella) + +# sepia +FetchContent_Declare( + sepia + GIT_REPOSITORY https://github.com/neuromorphic-paris/sepia + GIT_TAG master +) +FetchContent_MakeAvailable(sepia) + +# Eigen +find_package(Eigen3 3.3 REQUIRED NO_MODULE) + +# OpenCV +find_package(OpenCV) +if(${OPENCV_FOUND}) + message(STATUS "OpenCV version found - ${OpenCV_VERSION}") +else() + message(STATUS "OpenCV not found") +endif() + +# Source +add_subdirectory(${${LIB_NAME}_SOURCE_DIR}) + +# Documentation +if(${LIB_NAME}_BUILD_DOC) + add_subdirectory(${${LIB_NAME}_DOC_DIR}) +endif() diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..1f619ee --- /dev/null +++ b/Dockerfile @@ -0,0 +1,40 @@ +FROM ubuntu:20.04 + +ARG DEBIAN_FRONTEND=noninteractive + +# Base dependencies +RUN apt-get update && \ + apt-get upgrade -y && \ + apt-get -y install build-essential cmake git doxygen graphviz pkg-config libeigen3-dev wget + +WORKDIR /root + +# Eigen +RUN git clone https://gitlab.com/libeigen/eigen.git && \ + cd eigen && \ + git checkout 27367017bd0aef15a67ce76b8e263a94c2508a1c && \ + cmake -S . -B build -DCMAKE_INSTALL_PREFIX=~/.local && \ + cmake --build build && \ + cmake --build build --target install + +# OpenCV +RUN git clone https://github.com/opencv/opencv.git && \ + cd opencv && \ + git checkout 82ac7ea23620fb13b7b6be225fa1b0e848f5e72d +RUN git clone https://github.com/opencv/opencv_contrib.git && \ + cd opencv_contrib && \ + git checkout c4027ab7f912a3053175477d41b1a62d0078bc5f +RUN cd opencv && \ + cmake -S . -B build -DOPENCV_EXTRA_MODULES_PATH=~/opencv_contrib/modules -DCMAKE_INSTALL_PREFIX=~/.local && \ + cmake --build build && \ + cmake --build build --target install + +# ETTCM +RUN mkdir ETTCM +COPY . ETTCM + +WORKDIR /root/ETTCM + +RUN cmake -S . -B build -DCMAKE_BUILD_TYPE=Release -DEigen3_DIR=~/.local/share/eigen3/cmake -DOpenCV_DIR=~/.local/lib/cmake/opencv4 && \ + cmake --build build && \ + make -C build doc diff --git a/LICENSE b/LICENSE new file mode 100755 index 0000000..0f55d62 --- /dev/null +++ b/LICENSE @@ -0,0 +1,360 @@ +Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International +Public License + +By exercising the Licensed Rights (defined below), You accept and agree +to be bound by the terms and conditions of this Creative Commons +Attribution-NonCommercial-ShareAlike 4.0 International Public License +("Public License"). To the extent this Public License may be +interpreted as a contract, You are granted the Licensed Rights in +consideration of Your acceptance of these terms and conditions, and the +Licensor grants You such rights in consideration of benefits the +Licensor receives from making the Licensed Material available under +these terms and conditions. + + +Section 1 -- Definitions. + + a. Adapted Material means material subject to Copyright and Similar + Rights that is derived from or based upon the Licensed Material + and in which the Licensed Material is translated, altered, + arranged, transformed, or otherwise modified in a manner requiring + permission under the Copyright and Similar Rights held by the + Licensor. For purposes of this Public License, where the Licensed + Material is a musical work, performance, or sound recording, + Adapted Material is always produced where the Licensed Material is + synched in timed relation with a moving image. + + b. Adapter's License means the license You apply to Your Copyright + and Similar Rights in Your contributions to Adapted Material in + accordance with the terms and conditions of this Public License. + + c. BY-NC-SA Compatible License means a license listed at + creativecommons.org/compatiblelicenses, approved by Creative + Commons as essentially the equivalent of this Public License. + + d. Copyright and Similar Rights means copyright and/or similar rights + closely related to copyright including, without limitation, + performance, broadcast, sound recording, and Sui Generis Database + Rights, without regard to how the rights are labeled or + categorized. For purposes of this Public License, the rights + specified in Section 2(b)(1)-(2) are not Copyright and Similar + Rights. + + e. Effective Technological Measures means those measures that, in the + absence of proper authority, may not be circumvented under laws + fulfilling obligations under Article 11 of the WIPO Copyright + Treaty adopted on December 20, 1996, and/or similar international + agreements. + + f. Exceptions and Limitations means fair use, fair dealing, and/or + any other exception or limitation to Copyright and Similar Rights + that applies to Your use of the Licensed Material. + + g. License Elements means the license attributes listed in the name + of a Creative Commons Public License. The License Elements of this + Public License are Attribution, NonCommercial, and ShareAlike. + + h. Licensed Material means the artistic or literary work, database, + or other material to which the Licensor applied this Public + License. + + i. Licensed Rights means the rights granted to You subject to the + terms and conditions of this Public License, which are limited to + all Copyright and Similar Rights that apply to Your use of the + Licensed Material and that the Licensor has authority to license. + + j. Licensor means the individual(s) or entity(ies) granting rights + under this Public License. + + k. NonCommercial means not primarily intended for or directed towards + commercial advantage or monetary compensation. For purposes of + this Public License, the exchange of the Licensed Material for + other material subject to Copyright and Similar Rights by digital + file-sharing or similar means is NonCommercial provided there is + no payment of monetary compensation in connection with the + exchange. + + l. Share means to provide material to the public by any means or + process that requires permission under the Licensed Rights, such + as reproduction, public display, public performance, distribution, + dissemination, communication, or importation, and to make material + available to the public including in ways that members of the + public may access the material from a place and at a time + individually chosen by them. + + m. Sui Generis Database Rights means rights other than copyright + resulting from Directive 96/9/EC of the European Parliament and of + the Council of 11 March 1996 on the legal protection of databases, + as amended and/or succeeded, as well as other essentially + equivalent rights anywhere in the world. + + n. You means the individual or entity exercising the Licensed Rights + under this Public License. Your has a corresponding meaning. + + +Section 2 -- Scope. + + a. License grant. + + 1. Subject to the terms and conditions of this Public License, + the Licensor hereby grants You a worldwide, royalty-free, + non-sublicensable, non-exclusive, irrevocable license to + exercise the Licensed Rights in the Licensed Material to: + + a. reproduce and Share the Licensed Material, in whole or + in part, for NonCommercial purposes only; and + + b. produce, reproduce, and Share Adapted Material for + NonCommercial purposes only. + + 2. Exceptions and Limitations. For the avoidance of doubt, where + Exceptions and Limitations apply to Your use, this Public + License does not apply, and You do not need to comply with + its terms and conditions. + + 3. Term. The term of this Public License is specified in Section + 6(a). + + 4. Media and formats; technical modifications allowed. The + Licensor authorizes You to exercise the Licensed Rights in + all media and formats whether now known or hereafter created, + and to make technical modifications necessary to do so. The + Licensor waives and/or agrees not to assert any right or + authority to forbid You from making technical modifications + necessary to exercise the Licensed Rights, including + technical modifications necessary to circumvent Effective + Technological Measures. For purposes of this Public License, + simply making modifications authorized by this Section 2(a) + (4) never produces Adapted Material. + + 5. Downstream recipients. + + a. Offer from the Licensor -- Licensed Material. Every + recipient of the Licensed Material automatically + receives an offer from the Licensor to exercise the + Licensed Rights under the terms and conditions of this + Public License. + + b. Additional offer from the Licensor -- Adapted Material. + Every recipient of Adapted Material from You + automatically receives an offer from the Licensor to + exercise the Licensed Rights in the Adapted Material + under the conditions of the Adapter's License You apply. + + c. No downstream restrictions. You may not offer or impose + any additional or different terms or conditions on, or + apply any Effective Technological Measures to, the + Licensed Material if doing so restricts exercise of the + Licensed Rights by any recipient of the Licensed + Material. + + 6. No endorsement. Nothing in this Public License constitutes or + may be construed as permission to assert or imply that You + are, or that Your use of the Licensed Material is, connected + with, or sponsored, endorsed, or granted official status by, + the Licensor or others designated to receive attribution as + provided in Section 3(a)(1)(A)(i). + + b. Other rights. + + 1. Moral rights, such as the right of integrity, are not + licensed under this Public License, nor are publicity, + privacy, and/or other similar personality rights; however, to + the extent possible, the Licensor waives and/or agrees not to + assert any such rights held by the Licensor to the limited + extent necessary to allow You to exercise the Licensed + Rights, but not otherwise. + + 2. Patent and trademark rights are not licensed under this + Public License. + + 3. To the extent possible, the Licensor waives any right to + collect royalties from You for the exercise of the Licensed + Rights, whether directly or through a collecting society + under any voluntary or waivable statutory or compulsory + licensing scheme. In all other cases the Licensor expressly + reserves any right to collect such royalties, including when + the Licensed Material is used other than for NonCommercial + purposes. + + +Section 3 -- License Conditions. + +Your exercise of the Licensed Rights is expressly made subject to the +following conditions. + + a. Attribution. + + 1. If You Share the Licensed Material (including in modified + form), You must: + + a. retain the following if it is supplied by the Licensor + with the Licensed Material: + + i. identification of the creator(s) of the Licensed + Material and any others designated to receive + attribution, in any reasonable manner requested by + the Licensor (including by pseudonym if + designated); + + ii. a copyright notice; + + iii. a notice that refers to this Public License; + + iv. a notice that refers to the disclaimer of + warranties; + + v. a URI or hyperlink to the Licensed Material to the + extent reasonably practicable; + + b. indicate if You modified the Licensed Material and + retain an indication of any previous modifications; and + + c. indicate the Licensed Material is licensed under this + Public License, and include the text of, or the URI or + hyperlink to, this Public License. + + 2. You may satisfy the conditions in Section 3(a)(1) in any + reasonable manner based on the medium, means, and context in + which You Share the Licensed Material. For example, it may be + reasonable to satisfy the conditions by providing a URI or + hyperlink to a resource that includes the required + information. + 3. If requested by the Licensor, You must remove any of the + information required by Section 3(a)(1)(A) to the extent + reasonably practicable. + + b. ShareAlike. + + In addition to the conditions in Section 3(a), if You Share + Adapted Material You produce, the following conditions also apply. + + 1. The Adapter's License You apply must be a Creative Commons + license with the same License Elements, this version or + later, or a BY-NC-SA Compatible License. + + 2. You must include the text of, or the URI or hyperlink to, the + Adapter's License You apply. You may satisfy this condition + in any reasonable manner based on the medium, means, and + context in which You Share Adapted Material. + + 3. You may not offer or impose any additional or different terms + or conditions on, or apply any Effective Technological + Measures to, Adapted Material that restrict exercise of the + rights granted under the Adapter's License You apply. + + +Section 4 -- Sui Generis Database Rights. + +Where the Licensed Rights include Sui Generis Database Rights that +apply to Your use of the Licensed Material: + + a. for the avoidance of doubt, Section 2(a)(1) grants You the right + to extract, reuse, reproduce, and Share all or a substantial + portion of the contents of the database for NonCommercial purposes + only; + + b. if You include all or a substantial portion of the database + contents in a database in which You have Sui Generis Database + Rights, then the database in which You have Sui Generis Database + Rights (but not its individual contents) is Adapted Material, + including for purposes of Section 3(b); and + + c. You must comply with the conditions in Section 3(a) if You Share + all or a substantial portion of the contents of the database. + +For the avoidance of doubt, this Section 4 supplements and does not +replace Your obligations under this Public License where the Licensed +Rights include other Copyright and Similar Rights. + + +Section 5 -- Disclaimer of Warranties and Limitation of Liability. + + a. UNLESS OTHERWISE SEPARATELY UNDERTAKEN BY THE LICENSOR, TO THE + EXTENT POSSIBLE, THE LICENSOR OFFERS THE LICENSED MATERIAL AS-IS + AND AS-AVAILABLE, AND MAKES NO REPRESENTATIONS OR WARRANTIES OF + ANY KIND CONCERNING THE LICENSED MATERIAL, WHETHER EXPRESS, + IMPLIED, STATUTORY, OR OTHER. THIS INCLUDES, WITHOUT LIMITATION, + WARRANTIES OF TITLE, MERCHANTABILITY, FITNESS FOR A PARTICULAR + PURPOSE, NON-INFRINGEMENT, ABSENCE OF LATENT OR OTHER DEFECTS, + ACCURACY, OR THE PRESENCE OR ABSENCE OF ERRORS, WHETHER OR NOT + KNOWN OR DISCOVERABLE. WHERE DISCLAIMERS OF WARRANTIES ARE NOT + ALLOWED IN FULL OR IN PART, THIS DISCLAIMER MAY NOT APPLY TO YOU. + + b. TO THE EXTENT POSSIBLE, IN NO EVENT WILL THE LICENSOR BE LIABLE + TO YOU ON ANY LEGAL THEORY (INCLUDING, WITHOUT LIMITATION, + NEGLIGENCE) OR OTHERWISE FOR ANY DIRECT, SPECIAL, INDIRECT, + INCIDENTAL, CONSEQUENTIAL, PUNITIVE, EXEMPLARY, OR OTHER LOSSES, + COSTS, EXPENSES, OR DAMAGES ARISING OUT OF THIS PUBLIC LICENSE OR + USE OF THE LICENSED MATERIAL, EVEN IF THE LICENSOR HAS BEEN + ADVISED OF THE POSSIBILITY OF SUCH LOSSES, COSTS, EXPENSES, OR + DAMAGES. WHERE A LIMITATION OF LIABILITY IS NOT ALLOWED IN FULL OR + IN PART, THIS LIMITATION MAY NOT APPLY TO YOU. + + c. The disclaimer of warranties and limitation of liability provided + above shall be interpreted in a manner that, to the extent + possible, most closely approximates an absolute disclaimer and + waiver of all liability. + + +Section 6 -- Term and Termination. + + a. This Public License applies for the term of the Copyright and + Similar Rights licensed here. However, if You fail to comply with + this Public License, then Your rights under this Public License + terminate automatically. + + b. Where Your right to use the Licensed Material has terminated under + Section 6(a), it reinstates: + + 1. automatically as of the date the violation is cured, provided + it is cured within 30 days of Your discovery of the + violation; or + + 2. upon express reinstatement by the Licensor. + + For the avoidance of doubt, this Section 6(b) does not affect any + right the Licensor may have to seek remedies for Your violations + of this Public License. + + c. For the avoidance of doubt, the Licensor may also offer the + Licensed Material under separate terms or conditions or stop + distributing the Licensed Material at any time; however, doing so + will not terminate this Public License. + + d. Sections 1, 5, 6, 7, and 8 survive termination of this Public + License. + + +Section 7 -- Other Terms and Conditions. + + a. The Licensor shall not be bound by any additional or different + terms or conditions communicated by You unless expressly agreed. + + b. Any arrangements, understandings, or agreements regarding the + Licensed Material not stated herein are separate from and + independent of the terms and conditions of this Public License. + + +Section 8 -- Interpretation. + + a. For the avoidance of doubt, this Public License does not, and + shall not be interpreted to, reduce, limit, restrict, or impose + conditions on any use of the Licensed Material that could lawfully + be made without permission under this Public License. + + b. To the extent possible, if any provision of this Public License is + deemed unenforceable, it shall be automatically reformed to the + minimum extent necessary to make it enforceable. If the provision + cannot be reformed, it shall be severed from this Public License + without affecting the enforceability of the remaining terms and + conditions. + + c. No term or condition of this Public License will be waived and no + failure to comply consented to unless expressly agreed to by the + Licensor. + + d. Nothing in this Public License constitutes or may be interpreted + as a limitation upon, or waiver of, any privileges and immunities + that apply to the Licensor or You, including from the legal + processes of any jurisdiction or authority. diff --git a/README.md b/README.md new file mode 100644 index 0000000..d3daf93 --- /dev/null +++ b/README.md @@ -0,0 +1,134 @@ +# ETTCM + +[![License: CC BY-NC-SA 4.0](https://img.shields.io/badge/License-CC%20BY--NC--SA%204.0-lightgrey.svg?style=flat-square)](https://creativecommons.org/licenses/by-nc-sa/4.0/) + +Code for [Time-to-Contact Map by Joint Estimation of Up-to-Scale Inverse Depth and Global Motion using a Single Event Camera, ICCV 2023](https://openaccess.thecvf.com/content/ICCV2023/html/Nunes_Time-to-Contact_Map_by_Joint_Estimation_of_Up-to-Scale_Inverse_Depth_and_ICCV_2023_paper.html) + +```bibtex +@inproceedings{nunesTimeToContact2023, + title = {Time-to-Contact Map by Joint Estimation of Up-to-Scale Inverse Depth and Global Motion using a Single Event Camera}, + booktitle = {Proceedings of the IEEE/CVF International Conference on Computer Vision (ICCV)}, + author = {Nunes, Urbano Miguel and Perrinet, Laurent Udo and Ieng, Sio-Hoi}, + year = {2023}, + pages = {23653-23663}, +} +``` + +The authors provide this code in the hope it will be useful for understanding the proposed method, as well as for reproducibility of the results. + +For more information and more open-source software please visit the neuromorphic-paris' Github page: . + +# Datasets + +We provide all the sequences evaluated in the paper ready to be used: [VL](https://spik.xyz/nc/index.php/s/ER6GMim9qPQRxaF) (104.2MB) or run on a terminal + +```bash +wget --header 'Host: spik.xyz' --header 'Sec-GPC: 1' 'https://spik.xyz/nc/index.php/s/ER6GMim9qPQRxaF/download/VL.zip' --output-document 'VL.zip' +``` + +The original data can be found [here](https://github.com/s-mcleod/ventral-landing-event-dataset). + +# Manual Installation + +This code was tested on Ubuntu 20.04 distro. + +## Dependencies + +For a complete list of the dependencies you can also refer to the [Dockerfile](./Dockerfile). + +- Base dependencies: + +```bash +sudo apt-get install build-essential cmake git graphviz pkg-config libeigen3-dev +``` + +- Specific version of [Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page#Download) that needs to be installed separately, e.g., in `.local` folder: + +```bash +git clone https://gitlab.com/libeigen/eigen.git && cd eigen && git checkout 27367017bd0aef15a67ce76b8e263a94c2508a1c +cmake -S . -B build -DCMAKE_INSTALL_PREFIX=~/.local +cmake --build build +cmake --build build --target install +``` + +- Specific version of [OpenCV](https://docs.opencv.org/trunk/d7/d9f/tutorial_linux_install.html): + +```bash +git clone https://github.com/opencv/opencv.git && cd opencv && git checkout 82ac7ea23620fb13b7b6be225fa1b0e848f5e72d +cd .. +git clone https://github.com/opencv/opencv_contrib.git && cd opencv_contrib && git checkout c4027ab7f912a3053175477d41b1a62d0078bc5f +cd ../opencv +cmake -S . -B build -DOPENCV_EXTRA_MODULES_PATH=~/opencv_contrib/modules -DCMAKE_INSTALL_PREFIX=~/.local +cmake --build build +cmake --build build --target install +``` + +## General + +After all the dependencies have been installed, to compile this code, assuming you are on the code directory and the specific versions of [Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page#Download) and [OpenCV](https://docs.opencv.org/trunk/d7/d9f/tutorial_linux_install.html) were installed on the `.local` folder, run on a terminal: + +```bash +cmake -S . -B build -DCMAKE_BUILD_TYPE=Release -DEigen3_DIR=~/.local/share/eigen3/cmake -DOpenCV_DIR=~/.local/lib/cmake/opencv4 -DETTCM_BUILD_DOC=OFF +cmake --build build +``` + +# Dockerfile + +Just install [Docker](https://www.docker.com/) and then build a container by running on a terminal: + +```bash +docker build -t . +``` + +This will take a while (~6100s or ~1h40m in a standard laptop) since it will build all the necessary packages, including [OpenCV](https://docs.opencv.org/trunk/d7/d9f/tutorial_linux_install.html). +After everything is built, to run inside the container, on a terminal run: + +```bash +docker run -it +``` + +# Documentation + +To build the documentation, you need to have [Doxygen](https://www.doxygen.nl/) installed: + +```bash +sudo apt-get install doxygen +``` + +Then, recompile: + +```bash +cmake -S . -B build -DCMAKE_BUILD_TYPE=Release -DEigen3_DIR=~/.local/share/eigen3/cmake -DOpenCV_DIR=~/.local/lib/cmake/opencv4 -DETTCM_BUILD_DOC=ON +cmake --build build +make -C build doc +``` + +The documentation is built by default using the [Docker](https://www.docker.com/) file. +It can be accessed inside the folder [doc](./build/doc/doxygen). + +# Experimental Evaluation + +For the following, we assume you are either inside the [Docker](https://www.docker.com/) container or in the root of the code after manually compiling. + +## Tab. 2 Partial Results + +Only the 2D-odd and 3D sequences provided are evaluated. +To get the partial results in terms of accuracy shown in Tab. 2 just run on a terminal: + +```bash +bash table_2_partial_results.sh +``` + +## Tab. 2 Complete Results + +You need to download the data provided [here](https://spik.xyz/nc/index.php/s/ER6GMim9qPQRxaF), uncompress the zip file and move it to the [datasets](./datasets) folder. +To get the results in terms of accuracy shown in Tab. 2 just run on a terminal: + +```bash +bash table_2_results.sh +``` + +# License + +The ETTCM code is licensed under [CC BY-NC-SA 4.0](https://creativecommons.org/licenses/by-nc-sa/4.0/). +Commercial usage is not permitted. diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt new file mode 100644 index 0000000..96556cb --- /dev/null +++ b/doc/CMakeLists.txt @@ -0,0 +1,28 @@ +# Check if Doxygen is installed +find_package(Doxygen REQUIRED) + +# Find public headers +file(GLOB_RECURSE ${LIB_NAME}_PUBLIC_HEADERS ${${LIB_NAME}_INCLUDE_DIR}/*.hpp) + +# Set doxygen variables +set(DOXYFILE_IN ${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile.in) +set(DOXYFILE_OUT ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) +set(DOXYGEN_OUTPUT_DIR ${CMAKE_CURRENT_BINARY_DIR}/doxygen) +set(DOXYGEN_INDEX_FILE ${DOXYGEN_OUTPUT_DIR}/xml/index.html) + +# Configure Doxyfile +configure_file(${DOXYFILE_IN} ${DOXYFILE_OUT} @ONLY) + +# Doxygen won't create this folder +file(MAKE_DIRECTORY ${DOXYGEN_OUTPUT_DIR}) + +# Add command and target +add_custom_command(OUTPUT ${DOXYGEN_INDEX_FILE} + DEPENDS ${${LIB_NAME}_PUBLIC_HEADERS} + COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYFILE_OUT} + MAIN_DEPENDENCY ${DOXYFILE_IN} + COMMENT "Generating documentation with doxygen" + VERBATIM) + +# General docs target +add_custom_target(doc DEPENDS ${DOXYGEN_INDEX_FILE}) diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in new file mode 100644 index 0000000..974082b --- /dev/null +++ b/doc/Doxyfile.in @@ -0,0 +1,11 @@ +OUTPUT_DIRECTORY = @DOXYGEN_OUTPUT_DIR@ +INPUT = @CMAKE_CURRENT_SOURCE_DIR@/../include @CMAKE_CURRENT_SOURCE_DIR@/main.dox +PROJECT_NAME = ETTCM +RECURSIVE = YES +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = YES +EXPAND_ONLY_PREDEF = YES +GENERATE_XML = YES +USE_MATHJAX = YES +PREDEFINED = __attribute__(x)= \ + ETTCM_FAST_EXP diff --git a/doc/main.dox b/doc/main.dox new file mode 100644 index 0000000..356872b --- /dev/null +++ b/doc/main.dox @@ -0,0 +1,4 @@ +/*! \mainpage ETTCM + * + * Experimental code part of the submission of the paper Time-to-Contact Map by Joint Estimation of Up-to-Scale Inverse Depth and Global Motion using a Single Event Camera to ICCV 2023. + */ diff --git a/include/ETTCM.hpp b/include/ETTCM.hpp new file mode 100644 index 0000000..7a32454 --- /dev/null +++ b/include/ETTCM.hpp @@ -0,0 +1,19 @@ +/** + * @file + * @brief Includes all headers for convenience. + */ + +#ifndef ETTCM_HPP +#define ETTCM_HPP + +// All include files +#include "ETTCM/assert.hpp" +#include "ETTCM/global_decay.hpp" +#include "ETTCM/global_motion_inverse_depth.hpp" +#include "ETTCM/leaky_sampling.hpp" +#include "ETTCM/motion_model.hpp" +#include "ETTCM/rectify.hpp" +#include "ETTCM/types.hpp" +#include "ETTCM/utils.hpp" + +#endif // ETTCM_HPP diff --git a/include/ETTCM/assert.hpp b/include/ETTCM/assert.hpp new file mode 100644 index 0000000..b6cdd51 --- /dev/null +++ b/include/ETTCM/assert.hpp @@ -0,0 +1,31 @@ +/** + * @file + * @brief Custom assert message for debug purposes. + */ + +#ifndef ETTCM_ASSERT_HPP +#define ETTCM_ASSERT_HPP + +#include +#include + +namespace ETTCM +{ +/// \cond +#define NO_OP + +#ifndef NDEBUG +#define ASSERT(condition, message) \ + if (!(condition)) \ + { \ + std::cerr << "Assertion `" #condition "` failed in " << __FILE__ \ + << " line " << __LINE__ << ": " << message << '\n'; \ + std::terminate(); \ + } +#else +#define ASSERT(condition, message) NO_OP +#endif +/// \endcond +} // namespace ETTCM + +#endif // ETTCM_ASSERT_HPP diff --git a/include/ETTCM/global_decay.hpp b/include/ETTCM/global_decay.hpp new file mode 100644 index 0000000..76a4b75 --- /dev/null +++ b/include/ETTCM/global_decay.hpp @@ -0,0 +1,217 @@ +/** + * @file + * @brief Global decay estimator implementation. + */ + +#ifndef ETTCM_GLOBAL_DECAY_HPP +#define ETTCM_GLOBAL_DECAY_HPP + +#include +#include + +#include "ETTCM/types.hpp" +#include "ETTCM/utils.hpp" + +namespace ETTCM +{ +/** + * @brief Global decay estimator. + * + * This class estimates the decay from a stream of events generated by a single + * motion. + * + * @tparam Event Type of event. + * @tparam EventToDecay Type of the handle to pass from an event to a decay. + * @tparam HandleDecay Type of the handle to further process the estimated + * decay. + */ +template +class GlobalDecay +{ + public: + /** + * @brief Constructs an instance to estimate the global decay from a stream of + * events. + * + * @param t_decay_first @copybrief t_decay_first_ + * @param event_to_decay @copybrief event_to_decay_ + * @param handle_decay @copybrief handle_decay_ + */ + GlobalDecay(const uint64_t t_decay_first, EventToDecay&& event_to_decay, + HandleDecay&& handle_decay) + : t_decay_first_(t_decay_first), + event_to_decay_(std::forward(event_to_decay)), + handle_decay_(std::forward(handle_decay)) + { + reset(); + } + /** + * @brief Deleted copy constructor. + */ + GlobalDecay(const GlobalDecay&) = delete; + /** + * @brief Default move constructor. + */ + GlobalDecay(GlobalDecay&&) = default; + /** + * @brief Deleted copy assignment operator. + */ + GlobalDecay& + operator=(const GlobalDecay&) = delete; + /** + * @brief Default move assignment operator. + */ + GlobalDecay& + operator=(GlobalDecay&&) = default; + /** + * @brief Default destructor. + */ + ~GlobalDecay() = default; + + /** + * @brief Returns the current timestamp \f$[\text{microseconds}]\f$. + * + * @return Current timestamp \f$[\text{microseconds}]\f$. + */ + uint64_t + t() const + { + return decay_.t; + } + + /** + * @brief Returns the current decay. + * + * @return Current decay. + */ + float + decay() const + { + return decay_.decay; + } + + /** + * @brief Returns the count of the incoming number of events. + * + * @return Count of the incoming number of events. + */ + float + n_decay() const + { + return decay_.n_decay; + } + + /** + * @brief Returns the event time decay \f$[\text{microseconds}]\f$. + * + * @return Event time decay \f$[\text{microseconds}]\f$. + */ + float + t_decay() const + { + return decay_.t_decay; + } + + /** + * @brief Returns the current event rate + * \f$[\text{events}/\text{microseconds}]\f$. + * + * @return Current event rate \f$[\text{events}/\text{microseconds}]\f$. + */ + float + rate() const + { + return decay_.rate; + } + + /** + * @brief Estimates the global decay one event at a time. + * + * This method estimates the decay in an event-by-event basis by estimating + * the event stream activity. + * + * @param event Incoming event. + */ + void + operator()(Event event) + { + decay_.decay = static_cast(1); + const float t_diff = + (event.t > decay_.t) ? static_cast(event.t - decay_.t) : 0; + if (t_diff > 0) + { + decay_.decay /= static_cast(1e-6) * t_diff * decay_.n_decay + + static_cast(1); + + decay_.n_decay *= decay_.decay; + decay_.t_decay = decay_.decay * decay_.t_decay + t_diff; + + decay_.t = event.t; + } + ++decay_.n_decay; + + decay_.rate = decay_.n_decay / decay_.t_decay; + + handle_decay_(event_to_decay_(event, decay_.decay, decay_.n_decay, + decay_.t_decay, decay_.rate)); + } + + /** + * @brief Resets the context. + */ + void + reset() + { + decay_.reset(t_decay_first_); + } + + protected: + /** + * @brief Initial time rate assumption to bootstrap the rate estimator + * \f$[\text{microseconds}]\f$. + */ + const uint64_t t_decay_first_; + + /** + * @brief Decay stucture. + * \sa ETTCM::Decay. + */ + Decay decay_; + + /** + * @brief Handle to pass from an event to a decay. + */ + EventToDecay event_to_decay_; + /** + * @brief Handle to further process the estimated decay. + */ + HandleDecay handle_decay_; +}; + +/** + * @brief Make function that creates an instance of ETTCM::GlobalDecay. + * + * @tparam Event Type of event. + * @tparam EventToDecay Type of the handle to pass from an event to a decay. + * @tparam HandleDecay Type of the handle to further process the estimated + * decay. + * + * @param t_decay_first Initial decay assumption to bootstrap the rate + * estimator \f$[\text{microseconds}]\f$. + * @param event_to_decay Handle to pass from an event to to a decay. + * @param handle_decay Handle to further process the estimated decay. + * + * @return Instance of ETTCM::GlobalDecay. + */ +template +inline GlobalDecay +make_global_decay(const uint64_t t_decay_first, EventToDecay&& event_to_decay, + HandleDecay&& handle_decay) +{ + return GlobalDecay( + t_decay_first, std::forward(event_to_decay), + std::forward(handle_decay)); +} +} // namespace ETTCM + +#endif // ETTCM_GLOBAL_DECAY_HPP diff --git a/include/ETTCM/global_motion_inverse_depth.hpp b/include/ETTCM/global_motion_inverse_depth.hpp new file mode 100644 index 0000000..5e366f3 --- /dev/null +++ b/include/ETTCM/global_motion_inverse_depth.hpp @@ -0,0 +1,686 @@ +/** + * @file + * @brief Global (up-to scale) 3D motion and inverse depth estimator + * implementation. + */ + +#ifndef ETTCM_GLOBAL_MOTION_INVERSE_DEPTH_HPP +#define ETTCM_GLOBAL_MOTION_INVERSE_DEPTH_HPP + +#include +#include +#include +#include + +#include "ETTCM/assert.hpp" +#include "ETTCM/global_decay.hpp" +#include "ETTCM/motion_model.hpp" +#include "ETTCM/types.hpp" +#include "ETTCM/utils.hpp" + +namespace ETTCM +{ +/** + * @brief Global motion parameters. + */ +struct GlobalMotionParams +{ + /** + * @brief Theshold to stop the iterative estimation. + */ + float d_min; + /** + * @brief Maximum number of iterations of the iterative estimation. + */ + uint16_t number_iterations; +}; + +/** + * @brief Timestamp and weight of an event. + */ +struct Cell +{ + /** + * @brief Event timestamp \f$[\text{seconds}]\f$. + */ + float t; + /** + * @brief Event weight. + */ + float w; +}; + +/** + * @brief Global (up-to scale) 3D motion and inverse depth estimator. + * + * @tparam Event Type of event. + * @tparam MotionModel Type of global motion model to be estimated. + * @tparam RectifyMap Type of the mapping for event rectification. + * @tparam EventToGlobalMotion Type of the handle that passes from an event to + * global motion parameters. + * @tparam HandleGlobalMotion Type of the handle to further process the + * estimated global motion. + */ +template +class GlobalMotionInverseDepth +{ + public: + /// \cond + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// \endcond + + /** + * @brief Type of the context, which consist on a surface whose elements + * contain the most recent events. + */ + typedef StdVector Context; + + enum : uint8_t + { + NDims = MotionModel::NDims, /**< Number of spatial dimensions of the motion + model. */ + NV = MotionModel::NV, /**< Number of parameters of the linear velocity.*/ + NVars = + MotionModel::NVars, /**< Number of parameters of the motion model. */ + NModel = NVars + 1 /**< Number of parameters of the model. */ + }; + + /** + * @brief Constructs an instance to compute the global (up-to scale) 3D motion + * parameters and inverse depth from an event stream. + * + * @param spatial_window @copybrief spatial_window_ + * @param weight_thresh @copybrief weight_thresh_ + * @param motion_vars_prior Motion uncertainty prior \f$\geq0\f$. + * @param lambda_prior Parameterized inverse depth prior \f$\geq0\f$. + * @param decay @copybrief decay_ + * @param params @copybrief params_ + * @param rectify_map @copybrief rectify_map_ + * @param event_to_global_motion @copybrief event_to_global_motion_ + * @param handle_global_motion @copybrief handle_global_motion_ + */ + GlobalMotionInverseDepth(const uint16_t spatial_window, + const float weight_thresh, + const float motion_vars_prior, + const float lambda_prior, const Decay& decay, + const GlobalMotionParams& params, + const RectifyMap& rectify_map, + EventToGlobalMotion&& event_to_global_motion, + HandleGlobalMotion&& handle_global_motion) + : width_(rectify_map.width()), + height_(rectify_map.height()), + spatial_window_(spatial_window), + window_size_((spatial_window_ << 1) + 1), + weight_thresh_(weight_thresh), + params_(params), + motion_vars_(Vector::Zero()), + inverse_depth_(Matrix::Ones(width_, height_)), + context_on_(width_ * height_, Cell{-1, 0}), + context_off_(width_ * height_, Cell{-1, 0}), + decay_(decay), + rectify_map_(rectify_map), + event_to_global_motion_( + std::forward(event_to_global_motion)), + handle_global_motion_( + std::forward(handle_global_motion)) + { + ASSERT(0 <= weight_thresh_ && weight_thresh_ < 1, + "The weight threshold " << weight_thresh_ + << " must be between 0 and 1"); + ASSERT(0 <= motion_vars_prior, "The motion prior " + << motion_vars_prior + << " must be greater than 0"); + ASSERT(0 <= lambda_prior, + "The lambda prior " << lambda_prior << " must be greater than 0"); + + prior_.template head().array() = motion_vars_prior; + prior_(NVars) = lambda_prior; + } + /** + * @brief Deleted copy constructor. + */ + GlobalMotionInverseDepth(const GlobalMotionInverseDepth&) = delete; + /** + * @brief Default move constructor. + */ + GlobalMotionInverseDepth(GlobalMotionInverseDepth&&) = default; + /** + * @brief Deleted copy assignment operator. + */ + GlobalMotionInverseDepth& + operator=(const GlobalMotionInverseDepth&) = delete; + /** + * @brief Default move assignment operator. + */ + GlobalMotionInverseDepth& + operator=(GlobalMotionInverseDepth&&) = default; + /** + * @brief Default destructor. + */ + ~GlobalMotionInverseDepth() = default; + + /** + * @brief Incrementally computes the (up-to scale) translation in 3D motion + * parameters one event at a time and respective inverse depth. + * + * This method incrementally estimates the (up-to scale) translation in 3D + * motion parameters and inverse depth for each event in an event-by-event + * basis. + * + * @param event Incoming event. + */ + void + operator()(Event event) + { + ASSERT(event.x_original < width_, + "The event x coordinate " << event.x_original + << " must be smaller than " << width_); + ASSERT(event.y_original < height_, + "The event y coordinate " << event.y_original + << " must be smaller than " << height_); + + const Matrix& intrinsics = rectify_map_.intrinsics(); + Context& context = (event.is_increase) ? context_on_ : context_off_; + const float t = static_cast(1e-6) * static_cast(event.t); + + // compute specific incremental b matrix + const Vector p(event.x - intrinsics(0, 2), + event.y - intrinsics(1, 2)); + Vector v; + float inverse_depth = 0; + if constexpr (std::is_same::value) + { + { + const Matrix b(MotionModel::b_matrix_param( + p, intrinsics.diagonal().template head())); + v.noalias() = b * motion_vars_.template head(); + } + v.normalize(); + + Vector p_prev(event.x_original, event.y_original); + p_prev -= + (static_cast(spatial_window_) * v).template cast(); + + // local window boundaries + uint16_t x_min, x_max; + uint16_t y_min, y_max; + compute_local_window_boundaries(p_prev(0), p_prev(1), width_, height_, + spatial_window_, spatial_window_, x_min, + x_max, y_min, y_max); + + if (x_max >= x_min && y_max >= y_min) + { + float w_inverse_depth = 0; + // update context weights + for (uint16_t x = x_min; x <= x_max; ++x) + { + for (uint16_t y = y_min; y <= y_max; ++y) + { + Cell& context_cell = context[row_column_to_ind(x, y, height_)]; + if (context_cell.t >= 0) + { + const float t_diff = t - context_cell.t; + context_cell.w = + static_cast(1) / + (t_diff * decay_.n_decay + static_cast(1)); + + // if (context_cell.w >= weight_thresh_) + { + inverse_depth += context_cell.w * inverse_depth_(x, y); + w_inverse_depth += context_cell.w; + } + } + } + } + if (w_inverse_depth > 0) + { + inverse_depth /= w_inverse_depth; + } + else + { + inverse_depth = 1; + } + motion_vars_(NVars) = std::log(inverse_depth); + + // iterative estimation + Vector v3( + (motion_vars_(0) - motion_vars_(4)) / inverse_depth, + (motion_vars_(1) + motion_vars_(3)) / inverse_depth, + motion_vars_(2)); + const Vector motion_vars(motion_vars_); + Vector num; + Matrix den; + float d = params_.d_min; + for (uint16_t k = 0; + k < params_.number_iterations && d >= params_.d_min; ++k) + { + float w_sum = 0; + num.setZero(); + den.setZero(); + + for (uint16_t x = x_min; x <= x_max; ++x) + { + for (uint16_t y = y_min; y <= y_max; ++y) + { + const Cell& context_cell = + context[row_column_to_ind(x, y, height_)]; + const float t_diff = t - context_cell.t; + + if (t_diff > 0 && context_cell.w >= weight_thresh_) + { + const CvVector& p_map = rectify_map_.map(x, y); + const Vector p_centered( + p_map[0] - intrinsics(0, 2), p_map[1] - intrinsics(1, 2)); + + Matrix jac; + jac.template leftCols() = MotionModel::jacobian( + t_diff, p_centered, + intrinsics.diagonal().template head(), + inverse_depth); + { + const Matrix b(MotionModel::b_matrix( + p_centered, intrinsics.diagonal().template head(), + inverse_depth)); + jac.col(NVars).noalias() = b.template leftCols() * v3; + } + + const Vector p_diff(event.x - p_map[0], + event.y - p_map[1]); + const Matrix b(MotionModel::b_matrix_param( + p_centered, intrinsics.diagonal().template head(), + inverse_depth)); + const Vector residual( + p_diff - + t_diff * (b * motion_vars_.template head())); + + const float w = + context_cell.w * + compute_exp(-static_cast(0.5) * + (residual.transpose() * residual)(0)); + w_sum += w; + num.noalias() -= w * (jac.transpose() * residual); + den.noalias() += (w * jac.transpose()) * jac; + } + } + } + + if (w_sum > 0) + { + num.array() += + prior_.array() * (motion_vars - motion_vars_).array(); + den.diagonal() += prior_; + const Vector motion_vars_diff(den.llt().solve(num)); + motion_vars_ += motion_vars_diff; + inverse_depth = std::exp(motion_vars_(NVars)); + v3 << (motion_vars_(0) - motion_vars_(4)) / inverse_depth, + (motion_vars_(1) + motion_vars_(3)) / inverse_depth, + motion_vars_(2); + const float v_norm = v3.norm(); + if (v_norm > static_cast(1)) + { + v3 /= v_norm; + motion_vars_(0) = inverse_depth * v3(0) + motion_vars_(4); + motion_vars_(1) = inverse_depth * v3(1) - motion_vars_(3); + motion_vars_(2) = v3(2); + } + d = motion_vars_diff.norm(); + } + else + { + d = 0; + } + } + + inverse_depth_(event.x_original, event.y_original) = inverse_depth; + } + + // compute local flow + const Matrix b(MotionModel::b_matrix_param( + p, intrinsics.diagonal().template head(), inverse_depth)); + v.noalias() = b * motion_vars_.template head(); + } + else + { + { + const Matrix b(MotionModel::b_matrix( + p, intrinsics.diagonal().template head())); + v.noalias() = b * motion_vars_.template head(); + } + v.normalize(); + + Vector p_prev(event.x_original, event.y_original); + p_prev -= + (static_cast(spatial_window_) * v).template cast(); + + // local window boundaries + uint16_t x_min, x_max; + uint16_t y_min, y_max; + compute_local_window_boundaries(p_prev(0), p_prev(1), width_, height_, + spatial_window_, spatial_window_, x_min, + x_max, y_min, y_max); + + if (x_max >= x_min && y_max >= y_min) + { + float w_inverse_depth = 0; + // update context weights + for (uint16_t x = x_min; x <= x_max; ++x) + { + for (uint16_t y = y_min; y <= y_max; ++y) + { + Cell& context_cell = context[row_column_to_ind(x, y, height_)]; + if (context_cell.t >= 0) + { + const float t_diff = t - context_cell.t; + context_cell.w = + static_cast(1) / + (t_diff * decay_.n_decay + static_cast(1)); + + // if (context_cell.w >= weight_thresh_) + { + inverse_depth += context_cell.w * inverse_depth_(x, y); + w_inverse_depth += context_cell.w; + } + } + } + } + if (w_inverse_depth > 0) + { + inverse_depth /= w_inverse_depth; + } + else + { + inverse_depth = 1; + } + motion_vars_(NVars) = std::log(inverse_depth); + + // iterative estimation + const Vector motion_vars(motion_vars_); + Vector num; + Matrix den; + float d = params_.d_min; + for (uint16_t k = 0; + k < params_.number_iterations && d >= params_.d_min; ++k) + { + float w_sum = 0; + num.setZero(); + den.setZero(); + + for (uint16_t x = x_min; x <= x_max; ++x) + { + for (uint16_t y = y_min; y <= y_max; ++y) + { + const Cell& context_cell = + context[row_column_to_ind(x, y, height_)]; + const float t_diff = t - context_cell.t; + + if (t_diff > 0 && context_cell.w >= weight_thresh_) + { + const CvVector& p_map = rectify_map_.map(x, y); + const Vector p_centered( + p_map[0] - intrinsics(0, 2), p_map[1] - intrinsics(1, 2)); + + Matrix jac; + jac.template leftCols() = MotionModel::jacobian( + t_diff, p_centered, + intrinsics.diagonal().template head(), + inverse_depth); + jac.col(NVars).noalias() = jac.template leftCols() * + motion_vars_.template head(); + + const Vector p_diff(event.x - p_map[0], + event.y - p_map[1]); + const Vector residual( + p_diff + jac.template leftCols() * + motion_vars_.template head()); + + const float w = + context_cell.w * + compute_exp(-static_cast(0.5) * + (residual.transpose() * residual)(0)); + w_sum += w; + num.noalias() -= w * (jac.transpose() * residual); + den.noalias() += (w * jac.transpose()) * jac; + } + } + } + + if (w_sum > 0) + { + num.array() += + prior_.array() * (motion_vars - motion_vars_).array(); + den.diagonal() += prior_; + const Vector motion_vars_diff(den.llt().solve(num)); + motion_vars_ += motion_vars_diff; + const float v_norm = motion_vars_.template head().norm(); + if constexpr (std::is_same::value) + { + if (v_norm > static_cast(0)) + { + motion_vars_.template head() /= v_norm; + } + } + else + { + if (v_norm > static_cast(1)) + { + motion_vars_.template head() /= v_norm; + } + } + d = motion_vars_diff.norm(); + + inverse_depth = std::exp(motion_vars_(NVars)); + } + else + { + d = 0; + } + } + + inverse_depth_(event.x_original, event.y_original) = inverse_depth; + } + + // compute local flow + const Matrix b(MotionModel::b_matrix( + p, intrinsics.diagonal().template head(), inverse_depth)); + v.noalias() = b * motion_vars_.template head(); + } + + // add event to context + context[row_column_to_ind(event.x_original, event.y_original, height_)] = + Cell{t, 1}; + + handle_global_motion_(event_to_global_motion_( + event, motion_vars_.template head(), v, inverse_depth)); + } + + /** + * @brief Resets the context. + */ + void + reset() + { + motion_vars_.setZero(); + inverse_depth_.setOnes(); + context_on_.clear(); + context_on_.resize(width_ * height_, Cell{-1, 0}); + context_off_.clear(); + context_off_.resize(width_ * height_, Cell{-1, 0}); + } + + protected: + /** + * @brief Width of the context. + */ + const uint16_t width_; + /** + * @brief Height of the context. + */ + const uint16_t height_; + /** + * @brief Local window radius \f$[\text{pixel}]\f$. + */ + const uint16_t spatial_window_; + /** + * @brief Local window size \f$[\text{pixel}]\f$. + */ + const uint16_t window_size_; + /** + * @brief Weight threshold \f$[0,1)\f$. + */ + const float weight_thresh_; + + /** + * @brief Iterative estimation parameters. + * \sa ETTCM::GlobalMotionParams + */ + const GlobalMotionParams params_; + + /** + * @brief Global motion variables estimate. + */ + Vector motion_vars_; + + /** + * @brief Inverse depth estimates. + */ + Matrix inverse_depth_; + + /** + * @brief Uncertainty prior. + */ + Vector prior_; + + /** + * @brief Context with the most recent events with ON polarity. + */ + Context context_on_; + /** + * @brief Context with the most recent events with OFF polarity. + */ + Context context_off_; + + /** + * @brief Reference to a variable storing a global decay estimate. + * \sa ETTCM::Decay. + */ + const Decay& decay_; + + /** + * @brief Rectification map. + * \sa ETTCM::Rectify. + */ + const RectifyMap& rectify_map_; + + /** + * @brief Handle that passes from an event to global motion parameters. + */ + EventToGlobalMotion event_to_global_motion_; + /** + * @brief Handle to further process the estimated global motion + * parameters. + */ + HandleGlobalMotion handle_global_motion_; +}; + +/** + * @brief Make function that creates an instance of + * ETTCM::GlobalMotionInverseDepth. + * + * @tparam Event Type of event. + * @tparam MotionModel Type of global motion model to be estimated. + * @tparam RectifyMap Type of the mapping for event rectification. + * @tparam EventToGlobalMotion Type of the handle that passes from an event to + * global motion parameters. + * @tparam HandleGlobalMotion Type of the handle to further process the + * estimated global motion. + * + * @param spatial_window Local window radius \f$[\text{pixel}]\f$. + * @param weight_thresh Weight threshold \f$[0,1)\f$. + * @param motion_vars_prior Motion uncertainty prior \f$\geq0\f$. + * @param lambda_prior Parameterized inverse depth uncertainty prior + * \f$\geq0\f$. + * @param decay Reference to a variable storing a global decay estimate. + * \sa ETTCM::Decay. + * @param params Iterative estimation parameters. + * \sa ETTCM::GlobalMotionParams. + * @param rectify_map Rectification map. + * \sa ETTCM::Rectify. + * @param event_to_global_motion Handle that passes from an event to global + * motion parameters. + * @param handle_global_motion Handle to further process the estimated global + * motion parameters. + * + * @return Instance of ETTCM::GlobalMotionInverseDepth. + */ +template +inline GlobalMotionInverseDepth +make_global_motion_inverse_depth(const uint16_t spatial_window, + const float weight_thresh, + const float motion_vars_prior, + const float lambda_prior, const Decay& decay, + const GlobalMotionParams& params, + const RectifyMap& rectify_map, + EventToGlobalMotion&& event_to_global_motion, + HandleGlobalMotion&& handle_global_motion) +{ + return GlobalMotionInverseDepth( + spatial_window, weight_thresh, motion_vars_prior, lambda_prior, decay, + params, rectify_map, + std::forward(event_to_global_motion), + std::forward(handle_global_motion)); +} + +/** + * @brief Make function that creates an instance of + * ETTCM::GlobalMotionInverseDepth. + * + * @tparam Event Type of event. + * @tparam MotionModel Type of global motion model to be estimated. + * @tparam RectifyMap Type of the mapping for event rectification. + * @tparam EventToGlobalMotion Type of the handle that passes from an event to + * global motion parameters. + * @tparam HandleGlobalMotion Type of the handle to further process the + * estimated global motion. + * + * @param spatial_window Local window radius \f$[\text{pixel}]\f$. + * @param weight_thresh Weight threshold \f$[0,1)\f$. + * @param motion_vars_prior Motion uncertainty prior \f$\geq0\f$. + * @param lambda_prior Parameterized inverse depth uncertainty prior + * \f$\geq0\f$. + * @param decay Reference to a variable storing a global decay estimate. + * \sa ETTCM::Decay. + * @param rectify_map Rectification map. + * \sa ETTCM::Rectify. + * @param event_to_global_motion Handle that passes from an event to global + * motion parameters. + * @param handle_global_motion Handle to further process the estimated global + * motion parameters. + * + * @return Instance of ETTCM::GlobalMotionInverseDepth. + */ +template +inline GlobalMotionInverseDepth +make_global_motion_inverse_depth(const uint16_t spatial_window, + const float weight_thresh, + const float motion_vars_prior, + const float lambda_prior, const Decay& decay, + const RectifyMap& rectify_map, + EventToGlobalMotion&& event_to_global_motion, + HandleGlobalMotion&& handle_global_motion) +{ + return GlobalMotionInverseDepth( + spatial_window, weight_thresh, motion_vars_prior, lambda_prior, decay, + {static_cast(1.0e-4), 2}, rectify_map, + std::forward(event_to_global_motion), + std::forward(handle_global_motion)); +} +} // namespace ETTCM + +#endif // ETTCM_GLOBAL_MOTION_INVERSE_DEPTH_HPP diff --git a/include/ETTCM/leaky_sampling.hpp b/include/ETTCM/leaky_sampling.hpp new file mode 100644 index 0000000..2e83802 --- /dev/null +++ b/include/ETTCM/leaky_sampling.hpp @@ -0,0 +1,271 @@ +/** + * @file + * @brief Sampling by leaky-firing implementation. + */ + +#ifndef ETTCM_LEAKY_SAMPLING_HPP +#define ETTCM_LEAKY_SAMPLING_HPP + +#include +#include + +#include "ETTCM/assert.hpp" +#include "ETTCM/types.hpp" +#include "ETTCM/utils.hpp" + +namespace ETTCM +{ +/** + * @brief Sampling by leaky-firing. + * + * This class samples events based on a simple leaky-firing model. + * + * @tparam Event Type of event. + * @tparam EventToLeakyEvent Type of the handle that passes from an event to a + * leaky event. + * @tparam HandleLeakyEvent Type of the handle to further process the leaky + * events. + */ +template +class LeakySampling +{ + public: + /** + * @brief Type of the leaky-firing context. + */ + typedef Matrix Context; + + /** + * @brief Constructs an instance to sample events by a simple leaky-firing + * model. + * + * @param width Original context width. + * @param height Original context height. + * @param width_sampling_factor @copybrief width_sampling_factor_ + * @param height_sampling_factor @copybrief height_sampling_factor_ + * @param sampling_factor @copybrief sampling_factor_ + * @param event_to_leaky_event @copybrief event_to_leaky_event_ + * @param handle_leaky_event @copybrief handle_leaky_event_ + */ + LeakySampling(const uint16_t width, const uint16_t height, + const float width_sampling_factor, + const float height_sampling_factor, const float sampling_factor, + EventToLeakyEvent&& event_to_leaky_event, + HandleLeakyEvent&& handle_leaky_event) + : width_sampling_factor_(width_sampling_factor), + height_sampling_factor_(height_sampling_factor), + sampling_factor_(sampling_factor), + width_(std::ceil(static_cast(width) / width_sampling_factor_)), + height_( + std::ceil(static_cast(height) / height_sampling_factor_)), + context_on_(Context::Zero(width_, height_)), + context_off_(Context::Zero(width_, height_)), + event_to_leaky_event_( + std::forward(event_to_leaky_event)), + handle_leaky_event_(std::forward(handle_leaky_event)) + { + ASSERT(1 <= width_sampling_factor_, + "The width sampling reduction factor " + << width_sampling_factor_ << " must be greater or equal to 1"); + ASSERT(1 <= height_sampling_factor_, + "The height sampling reduction factor " + << height_sampling_factor_ << " must be greater or equal to 1"); + } + /** + * @brief Deleted copy constructor. + */ + LeakySampling(const LeakySampling&) = delete; + /** + * @brief Default move constructor. + */ + LeakySampling(LeakySampling&&) = default; + /** + * @brief Deleted copy assignment operator. + */ + LeakySampling& + operator=(const LeakySampling&) = delete; + /** + * @brief Default move assignment operator. + */ + LeakySampling& + operator=(LeakySampling&&) = default; + /** + * @brief Default destructor. + */ + ~LeakySampling() = default; + + /** + * @brief Returns the width of the leaky-firing context. + * + * @return Width of the context. + */ + uint16_t + width() const + { + return width_; + } + /** + * @brief Returns the height of the leaky-firing context. + * + * @return Height of the context. + */ + uint16_t + height() const + { + return height_; + } + + /** + * @brief Samples incoming events based on a simple leaky-firing model. + * + * This method samples events based on a simple leaky-firing model in an + * event-by-event basis. + * + * @param event Incoming event. + */ + void + operator()(Event event) + { + const uint16_t x = event.x / width_sampling_factor_; + ASSERT(x < width_, "The event x-leaky coordinate " + << x << " must be smaller than " << width_); + const uint16_t y = event.y / height_sampling_factor_; + ASSERT(y < height_, "The event y-leaky coordinate " + << y << " must be smaller than " << height_); + + Context& context = (event.is_increase) ? context_on_ : context_off_; + ++context(x, y); + if (context(x, y) >= sampling_factor_) + { + context(x, y) -= sampling_factor_; + handle_leaky_event_(event_to_leaky_event_(event, x, y)); + } + } + + /** + * @brief Resets the context. + */ + void + reset() + { + context_on_.setZero(); + context_off_.setZero(); + } + + protected: + /** + * @brief Width sampling factor. + */ + const float width_sampling_factor_; + /** + * @brief Height sampling factor. + */ + const float height_sampling_factor_; + /** + * @brief Sampling factor. + */ + const float sampling_factor_; + + /** + * @brief Width of the leaky-firing context computed as the quotient of the + * original width of the context and width_sampling_factor_. + */ + const uint16_t width_; + /** + * @brief Height of the leaky-firing context computed as the quotient of the + * original height of the context and height_sampling_factor_. + */ + const uint16_t height_; + + /** + * @brief Leaky-firing context of events with ON polarity. + */ + Context context_on_; + /** + * @brief Leaky-firing context of events with OFF polarity. + */ + Context context_off_; + + /** + * @brief Handle that passes from an event to a leaky event. + */ + EventToLeakyEvent event_to_leaky_event_; + /** + * @brief Handle to further process the leaky event. + */ + HandleLeakyEvent handle_leaky_event_; +}; + +/** + * @brief Make function that creates an instance of + * ETTCM::LeakySampling. + * + * @tparam Event Type of event. + * @tparam EventToLeakyEvent Type of the handle that passes from an event a + * leaky event. + * @tparam HandleLeakyEvent Type of the handle to further process the leaky + * events. + * + * @param width Original context width. + * @param height Original context height. + * @param width_sampling_factor Width sampling factor. + * @param height_sampling_factor Height sampling factor. + * @param event_to_leaky_event Handle that passes from an event to a leaky + * event. + * @param handle_leaky_event Handle to further process the leaky event. + * + * @return Instance of ETTCM::LeakySampling. + */ +template +inline LeakySampling +make_leaky_sampling(const uint16_t width, const uint16_t height, + const float width_sampling_factor, + const float height_sampling_factor, + EventToLeakyEvent&& event_to_leaky_event, + HandleLeakyEvent&& handle_leaky_event) +{ + return LeakySampling( + width, height, width_sampling_factor, height_sampling_factor, + width_sampling_factor * height_sampling_factor, + std::forward(event_to_leaky_event), + std::forward(handle_leaky_event)); +} + +/** + * @brief Make function that creates an instance of + * ETTCM::LeakySampling. + * + * @tparam Event Type of event. + * @tparam EventToLeakyEvent Type of the handle that passes from an event a + * leaky event. + * @tparam HandleLeakyEvent Type of the handle to further process the leaky + * events. + * + * @param width Original context width. + * @param height Original context height. + * @param width_sampling_factor Width sampling factor. + * @param height_sampling_factor Height sampling factor. + * @param sampling_factor Sampling factor. + * @param event_to_leaky_event Handle that passes from an event to a leaky + * event. + * @param handle_leaky_event Handle to further process the leaky event. + * + * @return Instance of ETTCM::LeakySampling. + */ +template +inline LeakySampling +make_leaky_sampling(const uint16_t width, const uint16_t height, + const float width_sampling_factor, + const float height_sampling_factor, + const float sampling_factor, + EventToLeakyEvent&& event_to_leaky_event, + HandleLeakyEvent&& handle_leaky_event) +{ + return LeakySampling( + width, height, width_sampling_factor, height_sampling_factor, + sampling_factor, std::forward(event_to_leaky_event), + std::forward(handle_leaky_event)); +} +} // namespace ETTCM + +#endif // ETTCM_LEAKY_SAMPLING_HPP diff --git a/include/ETTCM/motion_model.hpp b/include/ETTCM/motion_model.hpp new file mode 100644 index 0000000..5a5d549 --- /dev/null +++ b/include/ETTCM/motion_model.hpp @@ -0,0 +1,508 @@ +/** + * @file + * @brief Motion models implementation. + */ + +#ifndef ETTCM_MOTION_MODEL_HPP +#define ETTCM_MOTION_MODEL_HPP + +#include + +#include "ETTCM/types.hpp" +#include "ETTCM/utils.hpp" + +namespace ETTCM +{ +/** + * @brief Scaling motion model. + */ +struct Scaling +{ + enum : uint8_t + { + NDims = 2, /**< Number of spatial dimensions. */ + NV = 1, /**< Number of linear velocity parameters. */ + NVars = 1 /**< Number of motion model parameters. */ + }; + + /** + * @brief Computes a specific matrix for the incremental motion model + * parameters estimation. + * + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * + * @return Specific matrix for the incremental motion model parameters + * estimation. + */ + static Matrix + b_matrix(const Ref> p, + [[maybe_unused]] const Ref> f) + { + return (Matrix() << -p(0), -p(1)).finished(); + } + /** + * @brief Computes a specific matrix for the incremental motion model + * parameters estimation. + * + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * @param inverse_depth Inverse depth parameter. + * + * @return Specific matrix for the incremental motion model parameters + * estimation. + */ + static Matrix + b_matrix(const Ref> p, + [[maybe_unused]] const Ref> f, + const float inverse_depth) + { + return (Matrix() << -inverse_depth * p(0), + -inverse_depth * p(1)) + .finished(); + } + + /** + * @brief Computes the Jacobian for the incremental motion model parameters + * estimation. + * + * @param t_diff Time difference. + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * + * @return Jacobian for the incremental motion model parameters estimation. + */ + static Matrix + jacobian(const float t_diff, const Ref> p, + [[maybe_unused]] const Ref> f) + { + return -t_diff * b_matrix(p, f); + } + /** + * @brief Computes the Jacobian for the incremental motion model parameters + * estimation, taking the inverse depth into account. + * + * @param t_diff Time difference. + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * @param inverse_depth Inverse depth parameter. + * + * @return Jacobian for the incremental motion model parameters estimation. + */ + static Matrix + jacobian(const float t_diff, const Ref> p, + [[maybe_unused]] const Ref> f, + const float inverse_depth) + { + return -t_diff * b_matrix(p, f, inverse_depth); + } +}; + +/** + * @brief Driving motion model. + */ +struct Driving +{ + enum : uint8_t + { + NDims = 2, /**< Number of spatial dimensions. */ + NV = 1, /**< Number of linear velocity parameters. */ + NVars = 2 /**< Number of motion model parameters. */ + }; + + /** + * @brief Computes a specific matrix for the incremental motion model + * parameters estimation. + * + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * + * @return Specific matrix for the incremental motion model parameters + * estimation. + */ + static Matrix + b_matrix(const Ref> p, + const Ref> f) + { + return (Matrix() << -p(0), f(0) + p(0) * p(0) / f(0), + -p(1), p(0) * p(1) / f(0)) + .finished(); + } + /** + * @brief Computes a specific matrix for the incremental motion model + * parameters estimation, taking the inverse depth into account. + * + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * @param inverse_depth Inverse depth parameter. + * + * @return Specific matrix for the incremental motion model parameters + * estimation. + */ + static Matrix + b_matrix(const Ref> p, + const Ref> f, const float inverse_depth) + { + return (Matrix() << -inverse_depth * p(0), + f(0) + p(0) * p(0) / f(0), -inverse_depth * p(1), + p(0) * p(1) / f(0)) + .finished(); + } + + /** + * @brief Computes the Jacobian for the incremental motion model parameters + * estimation. + * + * @param t_diff Time difference. + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * + * @return Jacobian for the incremental motion model parameters estimation. + */ + static Matrix + jacobian(const float t_diff, const Ref> p, + const Ref> f) + { + return -t_diff * b_matrix(p, f); + } + /** + * @brief Computes the Jacobian for the incremental motion model parameters + * estimation, taking the inverse depth into account. + * + * @param t_diff Time difference. + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * @param inverse_depth Inverse depth parameter. + * + * @return Jacobian for the incremental motion model parameters estimation. + */ + static Matrix + jacobian(const float t_diff, const Ref> p, + const Ref> f, const float inverse_depth) + { + return -t_diff * b_matrix(p, f, inverse_depth); + } +}; + +/** + * @brief Translation in 3D motion model. + */ +struct Translation3D +{ + enum : uint8_t + { + NDims = 2, /**< Number of spatial dimensions. */ + NV = 3, /**< Number of linear velocity parameters. */ + NVars = 3 /**< Number of motion model parameters. */ + }; + + /** + * @brief Computes a specific matrix for the incremental motion model + * parameters estimation, without taking the inverse depth into account. + * + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * + * @return Specific matrix for the incremental motion model parameters + * estimation. + */ + static Matrix + b_matrix(const Ref> p, + const Ref> f) + { + return (Matrix() << f(0), 0, -p(0), 0, f(1), -p(1)) + .finished(); + } + /** + * @brief Computes a specific matrix for the incremental motion model + * parameters estimation, taking the inverse depth into account. + * + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * @param inverse_depth Inverse depth parameter. + * + * @return Specific matrix for the incremental motion model parameters + * estimation. + */ + static Matrix + b_matrix(const Ref> p, + const Ref> f, const float inverse_depth) + { + return (Matrix() << inverse_depth * f(0), 0, + -inverse_depth * p(0), 0, inverse_depth * f(1), + -inverse_depth * p(1)) + .finished(); + } + + /** + * @brief Computes the Jacobian for the incremental motion model parameters + * estimation, without taking the inverse depth into account. + * + * @param t_diff Time difference. + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * + * @return Jacobian for the incremental motion model parameters estimation. + */ + static Matrix + jacobian(const float t_diff, const Ref> p, + const Ref> f) + { + return -t_diff * b_matrix(p, f); + } + /** + * @brief Computes the Jacobian for the incremental motion model parameters + * estimation, taking the inverse depth into account. + * + * @param t_diff Time difference. + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * @param inverse_depth Inverse depth parameter. + * + * @return Jacobian for the incremental motion model parameters estimation. + */ + static Matrix + jacobian(const float t_diff, const Ref> p, + const Ref> f, const float inverse_depth) + { + return -t_diff * b_matrix(p, f, inverse_depth); + } +}; + +/** + * @brief 6-DOF motion model. + */ +struct SixDOF +{ + enum : uint8_t + { + NDims = 2, /**< Number of spatial dimensions. */ + NV = 3, /**< Number of linear velocity parameters. */ + NVars = 6 /**< Number of motion model parameters. */ + }; + + /** + * @brief Computes a specific matrix for the incremental motion model + * parameters estimation, without taking the inverse depth into account. + * + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * + * @return Specific matrix for the incremental motion model parameters + * estimation. + */ + static Matrix + b_matrix(const Ref> p, + const Ref> f) + { + return (Matrix() << f(0), 0, -p(0), + -p(0) * p(1) / f(1), f(0) + p(0) * p(0) / f(0), -p(1) * f(0) / f(1), + 0, f(1), -p(1), -(f(1) + p(1) * p(1) / f(1)), p(0) * p(1) / f(0), + p(0) * f(1) / f(0)) + .finished(); + } + /** + * @brief Computes a specific matrix for the incremental motion model + * parameters estimation, taking the inverse depth into account. + * + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * @param inverse_depth Inverse depth parameter. + * + * @return Specific matrix for the incremental motion model parameters + * estimation. + */ + static Matrix + b_matrix(const Ref> p, + const Ref> f, const float inverse_depth) + { + return (Matrix() << inverse_depth * f(0), 0, + -inverse_depth * p(0), -p(0) * p(1) / f(1), + f(0) + p(0) * p(0) / f(0), -p(1) * f(0) / f(1), 0, + inverse_depth * f(1), -inverse_depth * p(1), + -(f(1) + p(1) * p(1) / f(1)), p(0) * p(1) / f(0), + p(0) * f(1) / f(0)) + .finished(); + } + + /** + * @brief Computes the Jacobian for the incremental motion model parameters + * estimation, without taking the inverse depth into account. + * + * @param t_diff Time difference. + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * + * @return Jacobian for the incremental motion model parameters estimation. + */ + static Matrix + jacobian(const float t_diff, const Ref> p, + const Ref> f) + { + return -t_diff * b_matrix(p, f); + } + /** + * @brief Computes the Jacobian for the incremental motion model parameters + * estimation, taking the inverse depth into account. + * + * @param t_diff Time difference. + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * @param inverse_depth Inverse depth parameter. + * + * @return Jacobian for the incremental motion model parameters estimation. + */ + static Matrix + jacobian(const float t_diff, const Ref> p, + const Ref> f, const float inverse_depth) + { + return -t_diff * b_matrix(p, f, inverse_depth); + } +}; + +/** + * @brief Parameterized 6-DOF motion model. + */ +struct ParameterizedSixDOF +{ + enum : uint8_t + { + NDims = 2, /**< Number of spatial dimensions. */ + NV = 3, /**< Number of linear velocity parameters. */ + NVars = 6 /**< Number of motion model parameters. */ + }; + + /** + * @brief Computes a specific matrix for the incremental motion model + * parameters estimation, without taking the inverse depth into account. + * + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * + * @return Specific matrix for the incremental motion model parameters + * estimation. + */ + static Matrix + b_matrix(const Ref> p, + const Ref> f) + { + return (Matrix() << f(0), 0, -p(0), + -p(0) * p(1) / f(1), f(0) + p(0) * p(0) / f(0), -p(1) * f(0) / f(1), + 0, f(1), -p(1), -(f(1) + p(1) * p(1) / f(1)), p(0) * p(1) / f(0), + p(0) * f(1) / f(0)) + .finished(); + } + /** + * @brief Computes a specific matrix for the incremental motion model + * parameters estimation, taking the inverse depth into account. + * + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * @param inverse_depth Inverse depth parameter. + * + * @return Specific matrix for the incremental motion model parameters + * estimation. + */ + static Matrix + b_matrix(const Ref> p, + const Ref> f, const float inverse_depth) + { + return (Matrix() << inverse_depth * f(0), 0, + -inverse_depth * p(0), -p(0) * p(1) / f(1), + f(0) + p(0) * p(0) / f(0), -p(1) * f(0) / f(1), 0, + inverse_depth * f(1), -inverse_depth * p(1), + -(f(1) + p(1) * p(1) / f(1)), p(0) * p(1) / f(0), + p(0) * f(1) / f(0)) + .finished(); + } + + /** + * @brief Computes a specific matrix for the incremental motion model + * parameters estimation, without taking the inverse depth into account. + * + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * + * @return Specific matrix for the incremental motion model parameters + * estimation. + */ + static Matrix + b_matrix_param(const Ref> p, + const Ref> f) + { + return (Matrix() << f(0), 0, -p(0), + -p(0) * p(1) / f(1), p(0) * p(0) / f(0), -p(1) * f(0) / f(1), 0, + f(1), -p(1), -p(1) * p(1) / f(1), p(0) * p(1) / f(0), + p(0) * f(1) / f(0)) + .finished(); + } + /** + * @brief Computes a specific matrix for the incremental motion model + * parameters estimation, taking the inverse depth into account. + * + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * @param inverse_depth Inverse depth parameter. + * + * @return Specific matrix for the incremental motion model parameters + * estimation. + */ + static Matrix + b_matrix_param(const Ref> p, + const Ref> f, + const float inverse_depth) + { + return (Matrix() << f(0), 0, -inverse_depth * p(0), + -p(0) * p(1) / f(1), p(0) * p(0) / f(0), -p(1) * f(0) / f(1), 0, + f(1), -inverse_depth * p(1), -p(1) * p(1) / f(1), + p(0) * p(1) / f(0), p(0) * f(1) / f(0)) + .finished(); + } + + /** + * @brief Computes the Jacobian for the incremental motion model parameters + * estimation, without taking the inverse depth into account. + * + * @param t_diff Time difference. + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * + * @return Jacobian for the incremental motion model parameters estimation. + */ + static Matrix + jacobian(const float t_diff, const Ref> p, + const Ref> f) + { + return -t_diff * (Matrix() << f(0), 0, -p(0), + -p(0) * p(1) / f(1), f(0) + p(0) * p(0) / f(0), + -p(1) * f(0) / f(1), 0, f(1), -p(1), + -(f(1) + p(1) * p(1) / f(1)), p(0) * p(1) / f(0), + p(0) * f(1) / f(0)) + .finished(); + } + /** + * @brief Computes the Jacobian for the incremental motion model parameters + * estimation, taking the inverse depth into account. + * + * @param t_diff Time difference. + * @param p Centered spatial coordinates. + * @param f Focal parameters. + * @param inverse_depth Inverse depth parameter. + * + * @return Jacobian for the incremental motion model parameters estimation. + */ + static Matrix + jacobian(const float t_diff, const Ref> p, + const Ref> f, const float inverse_depth) + { + return -t_diff * (Matrix() << f(0), 0, + -inverse_depth * p(0), -p(0) * p(1) / f(1), + f(0) + p(0) * p(0) / f(0), -p(1) * f(0) / f(1), 0, f(1), + -inverse_depth * p(1), -(f(1) + p(1) * p(1) / f(1)), + p(0) * p(1) / f(0), p(0) * f(1) / f(0)) + .finished(); + } +}; +} // namespace ETTCM + +#endif // ETTCM_MOTION_MODEL_HPP diff --git a/include/ETTCM/rectify.hpp b/include/ETTCM/rectify.hpp new file mode 100644 index 0000000..634447e --- /dev/null +++ b/include/ETTCM/rectify.hpp @@ -0,0 +1,234 @@ +/** + * @file + * @brief Rectify implementation. + */ + +#ifndef ETTCM_RECTIFY_HPP +#define ETTCM_RECTIFY_HPP + +#include +#include +#include +#include +#include + +#include "ETTCM/assert.hpp" +#include "ETTCM/types.hpp" +#include "ETTCM/utils.hpp" + +namespace ETTCM +{ +/** + * @brief Compute the rectify maps from a calibration file. + * + * This class computes the rectify maps from a calibration file. + * The calibration file should contain the camera intrinsics and distortion + * parameters, as well as the context size. + * \see read_calibration() for the exact format. + */ +class Rectify +{ + public: + /// \cond + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// \endcond + + /** + * @brief Constructs an instance to compute the rectify maps from a + * calibration file. + * + * @param calibration_filename Name of the calibration file. + * @param alpha Free scaling parameter between 0 and 1. + * See cv::getOptimalNewCameraMatrix() + * for details. + * @param width_sampling_factor Width sampling factor. + * @param height_sampling_factor Height sampling factor. + */ + Rectify(const std::string& calibration_filename, const float alpha, + const float width_sampling_factor, const float height_sampling_factor) + { + ASSERT(0 <= alpha && alpha <= 1, "The free scaling parameter " + << alpha + << " must be between 0 and 1"); + ASSERT(1 <= width_sampling_factor, "The width sampling reduction factor " + << width_sampling_factor + << " must be greater or equal to 1"); + ASSERT(1 <= height_sampling_factor, + "The height sampling reduction factor " + << height_sampling_factor << " must be greater or equal to 1"); + + // read calibration parameters + uint16_t width, height; + read_calibration(calibration_filename, width, height, original_intrinsics_, + distortion_parameters_); + width_ = std::ceil(static_cast(width) / width_sampling_factor); + height_ = std::ceil(static_cast(height) / height_sampling_factor); + + CvMatrix original_intrinsics; + cv::eigen2cv(original_intrinsics_, original_intrinsics); + original_intrinsics.row(0) /= width_sampling_factor; + original_intrinsics.row(1) /= height_sampling_factor; + CvMatrix distortion_parameters; + cv::eigen2cv(distortion_parameters_, distortion_parameters); + + // compute new optimal intrinsics + CvMatrix intrinsics = cv::getOptimalNewCameraMatrix( + original_intrinsics, distortion_parameters, CvSize(width_, height_), + alpha, CvSize(width_, height_)); + cv::cv2eigen(intrinsics, intrinsics_); + + // compute undistortion map + CvMatrix map; + cv::initInverseRectificationMap( + original_intrinsics, distortion_parameters, CvMatrix(), intrinsics, + CvSize(width_, height_), CV_32FC2, map_, map); + } + /** + * @brief Deleted copy constructor. + */ + Rectify(const Rectify&) = delete; + /** + * @brief Default move constructor. + */ + Rectify(Rectify&&) = default; + /** + * @brief Deleted copy assignment operator. + */ + Rectify& + operator=(const Rectify&) = delete; + /** + * @brief Default move assignment operator. + */ + Rectify& + operator=(Rectify&&) = default; + /** + * @brief Default destructor. + */ + ~Rectify() = default; + + /** + * @brief Returns the width of the context. + * + * @return Width of the context. + */ + uint16_t + width() const + { + return width_; + } + + /** + * @brief Returns the height of the context. + * + * @return Height of the context. + */ + uint16_t + height() const + { + return height_; + } + + /** + * @brief Returns the undistorted camera intrinsics. + * + * @return Reference to the undistorted camera intrinsics. + */ + const Matrix& + intrinsics() const + { + return intrinsics_; + } + + /** + * @brief Returns the original camera intrinsics. + * + * @return Reference to the original camera intrinsics. + */ + const Matrix& + original_intrinsics() const + { + return original_intrinsics_; + } + + /** + * @brief Returns the camera distortion parameters. + * + * @return Reference to the camera distortion parameters. + */ + const Vector& + distortion_parameters() const + { + return distortion_parameters_; + } + + /** + * @brief Returns the undistorted coordinates. + * + * @param x x-coordinate. + * @param y y-coordinate. + * @return Reference to the undistorted coordinates. + */ + const CvVector& + map(const uint16_t x, const uint16_t y) const + { + ASSERT(x < width_, + "The x coordinate " << x << " must be smaller than " << width_); + ASSERT(y < height_, + "The y coordinate " << y << " must be smaller than " << height_); + return map_.at>(y, x); + } + + protected: + /** + * @brief Width of the context. + */ + uint16_t width_; + /** + * @brief Height of the context. + */ + uint16_t height_; + + /** + * @brief Undistorted camera intrinsics. + */ + Matrix intrinsics_; + /** + * @brief Original camera intrinsics. + */ + Matrix original_intrinsics_; + /** + * @brief Camera distortion parameters. + */ + Vector distortion_parameters_; + + /** + * @brief Rectify maps. + */ + CvMatrix map_; +}; + +/** + * @brief Make function that creates an instance of ETTCM::Rectify. + * + * @param calibration_filename Name of the calibration file. + * @param alpha Free scaling parameter between 0 and 1. + * See cv::getOptimalNewCameraMatrix() + * for details. + * @param width_sampling_factor Width sampling factor. + * @param height_sampling_factor Height sampling factor. + * + * @return Instance of ETTCM::Rectify. + */ +inline Rectify +make_rectify(const std::string& calibration_filename, const float alpha = 0, + const float width_sampling_factor = 1, + const float height_sampling_factor = 1) +{ + return Rectify(calibration_filename, alpha, width_sampling_factor, + height_sampling_factor); +} +} // namespace ETTCM + +#endif // ETTCM_RECTIFY_HPP diff --git a/include/ETTCM/types.hpp b/include/ETTCM/types.hpp new file mode 100644 index 0000000..8a6c67b --- /dev/null +++ b/include/ETTCM/types.hpp @@ -0,0 +1,326 @@ +/** + * @file + * @brief Common types used by the library. + */ + +#ifndef ETTCM_TYPES_HPP +#define ETTCM_TYPES_HPP + +#include +#include +#include +#include +#include +#include + +namespace ETTCM +{ +/** + * @brief Alias type for std::vector. + */ +template > +using StdVector = typename std::vector; + +/** + * @brief Alias type for cv::Mat. + */ +typedef cv::Mat CvMatrix; +/** + * @brief Alias type for cv::Size. + */ +typedef cv::Size CvSize; +/** + * @brief Alias type for cv::Vec. + */ +template +using CvVector = cv::Vec; + +/// \cond +#define CV_TYPE(T, N) CV_MAKETYPE(cv::DataDepth::value, N) +/// \endcond + +// Eigen definitions +/// \cond +constexpr int Dynamic = Eigen::Dynamic; +/// \endcond + +/** + * @brief Alias type for Eigen::Array. + */ +template +using Array = typename Eigen::Array; +/** + * @brief Alias type for Eigen::Matrix. + */ +template +using Matrix = typename Eigen::Matrix; +/** + * @brief Alias type representing a row vector. + */ +template +using RowVector = typename Eigen::Matrix; +/** + * @brief Alias type representing a column vector. + */ +template +using Vector = typename Eigen::Matrix; + +/** + * @brief Alias type for Eigen::DenseBase. + */ +template +using DenseBase = typename Eigen::DenseBase; +/** + * @brief Alias type for Eigen::MatrixBase. + */ +template +using MatrixBase = typename Eigen::MatrixBase; +/** + * @brief Alias type for Eigen::PlainObjectBase. + */ +template +using PlainObjectBase = typename Eigen::PlainObjectBase; + +/** + * @brief Alias type for Eigen::Map. + */ +template +using Map = typename Eigen::Map; +/** + * @brief Alias type for Eigen::Ref. + */ +template +using Ref = typename Eigen::Ref; + +/** + * @brief Event decay structure. + */ +struct Decay +{ + /** + * @brief Previous timestamp \f$[\text{microseconds}]\f$. + */ + uint64_t t; + /** + * @brief Event decay in \f$[0,1]\f$. + */ + float decay; + /** + * @brief Auxiliary variable that counts the incoming number of events. + */ + float n_decay; + /** + * @brief Auxiliary variable that estimates the event time decay + * \f$[\text{microseconds}]\f$. + */ + float t_decay; + /** + * @brief Estimated event rate \f$[\text{events}/\text{microseconds}]\f$. + */ + float rate; + + /** + * @brief Resets the context. + * + * @param t_decay_first Initial time rate assumption to bootstrap the rate + * estimator \f$[\text{microseconds}]\f$. + */ + void + reset(const uint64_t t_decay_first) + { + t = 0; + decay = 1; + n_decay = 0; + t_decay = t_decay_first; + rate = 0; + } +}; + +/** + * @brief Structure representing a global motion and inverse depth event. + * + * @tparam MotionModel Type of global motion model. + */ +template +struct GlobalMotionInverseDepthEvent +{ + /// \cond + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// \endcond + + /** + * @brief Timestamp of the event. + */ + uint64_t t; + /** + * @brief Horizontal coordinate of the event before rectification. + */ + uint16_t x_original; + /** + * @brief Vertical coordinate of the event before rectification. + */ + uint16_t y_original; + /** + * @brief Horizontal coordinate of the event after rectification. + */ + float x; + /** + * @brief Vertical coordinate of the event after rectification. + */ + float y; + /** + * @brief Global motion parameters. + */ + Vector motion_vars; + /** + * @brief Local horizontal flow. + */ + float vx; + /** + * @brief Local vertical flow. + */ + float vy; + /** + * @brief Local inverse depth estimate. + */ + float inverse_depth; + /** + * @brief Polarity of the event. + */ + bool is_increase; +}; + +/** + * @brief Structure representing a global motion, inverse depth and + * time-to-contact event. + * + * @tparam MotionModel Type of global motion model. + */ +template +struct GlobalMotionInverseDepthTimeToContactEvent +{ + /// \cond + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// \endcond + + /** + * @brief Timestamp of the event. + */ + uint64_t t; + /** + * @brief Horizontal coordinate of the event before rectification. + */ + uint16_t x_original; + /** + * @brief Vertical coordinate of the event before rectification. + */ + uint16_t y_original; + /** + * @brief Horizontal coordinate of the event after rectification. + */ + float x; + /** + * @brief Vertical coordinate of the event after rectification. + */ + float y; + /** + * @brief Global motion parameters. + */ + Vector motion_vars; + /** + * @brief Time-to-contact parameter. + */ + float time_to_contact; + /** + * @brief Local horizontal flow. + */ + float vx; + /** + * @brief Local vertical flow. + */ + float vy; + /** + * @brief Local inverse depth estimate. + */ + float inverse_depth; + /** + * @brief Polarity of the event. + */ + bool is_increase; +}; + +/** + * @brief Structure representing a leaky event. + */ +struct __attribute__((__packed__)) LeakyEvent +{ + /** + * @brief Timestamp of the event. + */ + uint64_t t; + /** + * @brief Horizontal coordinate of the event before leakage. + */ + uint16_t x_original; + /** + * @brief Vertical coordinate of the event before leakage. + */ + uint16_t y_original; + /** + * @brief Horizontal coordinate of the event after leakage. + */ + uint16_t x; + /** + * @brief Vertical coordinate of the event after leakage. + */ + uint16_t y; + /** + * @brief Polarity of the event. + */ + bool is_increase; +}; + +/** + * @brief Structure representing an event rectified. + */ +struct __attribute__((__packed__)) RectifiedEvent +{ + /** + * @brief Timestamp of the event. + */ + uint64_t t; + /** + * @brief Horizontal coordinate of the event before rectification. + */ + uint16_t x_original; + /** + * @brief Vertical coordinate of the event before rectification. + */ + uint16_t y_original; + /** + * @brief Horizontal coordinate of the event after rectification. + */ + float x; + /** + * @brief Vertical coordinate of the event after rectification. + */ + float y; + /** + * @brief Polarity of the event. + */ + bool is_increase; +}; +} // namespace ETTCM + +#endif // ETTCM_TYPES_HPP diff --git a/include/ETTCM/utils.hpp b/include/ETTCM/utils.hpp new file mode 100644 index 0000000..ba79be1 --- /dev/null +++ b/include/ETTCM/utils.hpp @@ -0,0 +1,274 @@ +/** + * @file + * @brief Utility functions. + */ + +#ifndef ETTCM_UTILS_HPP +#define ETTCM_UTILS_HPP + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ETTCM/assert.hpp" +#include "ETTCM/types.hpp" +#include "pontella.hpp" +#include "sepia.hpp" + +namespace ETTCM +{ +/** + * @brief Computes the base-e exponential function. + * + * This function returns the value of e (the base of natural logarithm) raised + * to the power of \p x. + * + * @param x Input value. + * + * @return Exponential value of \p x. + */ +inline double +compute_exp(const double x) +{ +#ifdef ETTCM_FAST_EXP + if (x < -600) + { + return 0; + } + union + { + double d; + long l; + } u; + u.l = long(double((long(1) << 52) / 0.69314718056) * x + + double((long(1) << 52) * 1023)); + return u.d; +#else + return std::exp(x); +#endif +} + +/** + * @brief Computes the local window boundaries centered at the input + * coordinates. + * + * @param x x-coordinate. + * @param y y-coordinate. + * @param width Context width. + * @param height Context height. + * @param x_radius Local x-radius. + * @param y_radius Local y-radius. + * @param[out] x_min Minimum local x-coordinate. + * @param[out] x_max Maximum local x-coordinate. + * @param[out] y_min Minimum local y-coordinate. + * @param[out] y_max Maximum local y-coordinate. + */ +inline void +compute_local_window_boundaries(const uint16_t x, const uint16_t y, + const uint16_t width, const uint16_t height, + const uint16_t x_radius, + const uint16_t y_radius, uint16_t& x_min, + uint16_t& x_max, uint16_t& y_min, + uint16_t& y_max) +{ + x_min = std::max(static_cast(x) - static_cast(x_radius), 0); + y_min = std::max(static_cast(y) - static_cast(y_radius), 0); + x_max = std::min(static_cast(x) + static_cast(x_radius), + static_cast(width - 1)); + y_max = std::min(static_cast(y) + static_cast(y_radius), + static_cast(height - 1)); +} + +/** + * @brief Extracts the argument value from a command. + * + * This function returns the argument value from a \p command given its \p name. + * In case the argument \p name is not found, the default value is returned. + * + * @tparam T Type of the argument value. + * + * @param command Command line. + * @param name Name of the argument. + * @param default_argument Default argument value. + * + * @return Argument value if the argument \p name is found in the \p command. + * @return Passed argument value if the argument \p name is not found in the \p + * command. + */ +template +T +extract_argument(pontella::command command, const std::string& name, + T default_argument) +{ + const auto name_and_argument = command.options.find(name); + if (name_and_argument != command.options.end()) + { + std::stringstream name_stream(name_and_argument->second); + T argument; + name_stream >> argument; + return argument; + } + return default_argument; +} + +/** + * @brief Gets the file stream handle. + * + * @param filename Full path to the file. + * + * @return File stream handle. + */ +inline std::ifstream +filename_to_ifstream(const std::string& filename) +{ + auto stream = std::ifstream(filename, std::ifstream::in); + if (!stream.good()) + { + throw sepia::unreadable_file(filename); + } + return stream; +} + +/** + * @brief Reads the camera calibration parameters from a file. + * + * This function reads the camera calibration parameters from a file. + * The calibration file should contain the camera intrinsics and distortion + * parameters, as well as the context size in the following format: + * + * \f[f_{x}~f_{y}~c_{x}~c_{y}~d_{1}~d_{2}~d_{3}~d_{4}~d_{5}\f] + * + * \f[w~h,\f] + * + * where \f$w\f$ and \f$h\f$ are the width and height values of the context, + * respectively. + * + * @tparam T Numerical type of the matrix elements. + * + * @param filename Name of the calibration file. + * @param[out] width Width of the context \f$w\f$. + * @param[out] height Height of the context \f$h\f$. + * @param[out] intrinsics Camera intrinsics of the form + * \f$\begin{bmatrix} + * f_{x} & 0 & c_{x} \\ + * 0 & f_{y} & c_{y} \\ + * 0 & 0 & 1 + * \end{bmatrix}\f$. + * @param[out] distortion_parameters Camera distortion parameters of the form + * \f$\begin{bmatrix}d_{1} & d_{2} & d_{3} & d_{4} & d_{5}\end{bmatrix}\f$. + * + * @return Returns true if the file exists and false otherwise. + */ +template +void +read_calibration(const std::string& filename, uint16_t& width, uint16_t& height, + Matrix& intrinsics, + Vector& distortion_parameters) +{ + std::ifstream fin(filename_to_ifstream(filename)); + + // intrinsics + intrinsics.setIdentity(); + fin >> intrinsics(0, 0); + fin >> intrinsics(1, 1); + fin >> intrinsics(0, 2); + fin >> intrinsics(1, 2); + + // distortion parameters + for (uint16_t i = 0; i < 5; ++i) + { + fin >> distortion_parameters(i); + } + + // context size + fin >> width; + fin >> height; +} + +/** + * @brief Reads one value. + * + * @tparam T Type of the value. + * + * @param fin File stream handle. + * @param[out] val Last value. + */ +template +inline void +read_value(std::ifstream& fin, T& val) +{ + if (fin.eof()) + { + throw sepia::end_of_file(); + } + + fin >> val; +} + +/** + * @brief Reads one value. + * + * @tparam T Type of the value. + * + * @param filename File name. + * @param[out] val Last value. + */ +template +inline void +read_value(const std::string& filename, T& val) +{ + std::ifstream fin(filename_to_ifstream(filename)); + + read_value(fin, val); +} + +/** + * @brief Converts coordinates from 2D array to 1D array. + * + * @param row Row index. + * @param column Column index. + * @param height Height of the original 2D array. + * + * @return 1D array coordinate. + */ +inline uint64_t +row_column_to_ind(const uint16_t row, const uint16_t column, + const uint16_t height) +{ + return row * height + column; +} + +/** + * @brief String format. + * + * @tparam Args Types of the remaining arguments. + * + * @param format Input string format. + * @param args Remaining arguments. + * + * @return Formatted string. + */ +template +std::string +string_format(const std::string& format, Args... args) +{ + const int size_s = std::snprintf(nullptr, 0, format.c_str(), args...) + + 1; // extra space for '\0' + if (size_s <= 0) + { + throw std::runtime_error("Error during string formatting."); + } + const size_t size = static_cast(size_s); + auto buf = std::make_unique(size); + std::snprintf(buf.get(), size, format.c_str(), args...); + return std::string(buf.get(), + buf.get() + size - 1); // we don't want the '\0' inside +} +} // namespace ETTCM + +#endif // ETTCM_UTILS_HPP diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..103fc71 --- /dev/null +++ b/src/CMakeLists.txt @@ -0,0 +1,31 @@ +# Library +add_library(${LIB_NAME} INTERFACE) + +target_include_directories(${LIB_NAME} INTERFACE ${${LIB_NAME}_INCLUDE_DIR} + ${pontella_SOURCE_DIR}/source ${sepia_SOURCE_DIR}/source ${tarsier_SOURCE_DIR}/source) +target_link_libraries(${LIB_NAME} INTERFACE Eigen3::Eigen pthread stdc++fs) + +# OpenCV +if(${OPENCV_FOUND}) + target_compile_definitions(${LIB_NAME} INTERFACE ${LIB_NAME}_OPENCV_FOUND) + + target_include_directories(${LIB_NAME} INTERFACE ${OpenCV_INCLUDE_DIRS}) + target_link_libraries(${LIB_NAME} INTERFACE ${OpenCV_LIBS}) +endif() + +# Option for fast exp computation +option(${LIB_NAME}_FAST_EXP "Enable fast exp" ON) +if(${LIB_NAME}_FAST_EXP) + target_compile_definitions(${LIB_NAME} INTERFACE ${LIB_NAME}_FAST_EXP) +endif() + +# Function to add new executables +function(add_new_executable ARG) + add_executable(${ARG} ${ARG}.cpp) + target_link_libraries(${ARG} ${LIB_NAME}) +endfunction() + +# List of executables +add_new_executable(global_divergence_scaling) +add_new_executable(global_divergence_translation) +add_new_executable(global_divergence_six_dof) diff --git a/src/global_divergence.hpp b/src/global_divergence.hpp new file mode 100644 index 0000000..686cfc8 --- /dev/null +++ b/src/global_divergence.hpp @@ -0,0 +1,199 @@ +#ifndef ETTCM_GLOBAL_DIVERGENCE_HPP +#define ETTCM_GLOBAL_DIVERGENCE_HPP + +#include + +#include "ETTCM.hpp" +#include "pontella.hpp" +#include "sepia.hpp" + +#define GLOBAL_DIVERGENCE(MotionModel) \ + using namespace ETTCM; \ + \ + int main(int argc, char* argv[]) \ + { \ + typedef sepia::dvs_event Event; \ + typedef GlobalMotionInverseDepthEvent GlobalMotionEvent; \ + constexpr sepia::type Type = sepia::type::dvs; \ + \ + struct Arguments \ + { \ + uint16_t spatial_window; \ + uint64_t t_decay_first; \ + float weight_thresh; \ + float motion_vars_prior; \ + float lambda_prior; \ + float width_sampling_factor; \ + float height_sampling_factor; \ + }; \ + \ + return pontella::main( \ + {"global_divergence is an accuracy benchmark for global motion and " \ + "inverse depth global divergence estimation at provided timestamps", \ + "Usage: " \ + "./global_divergence [options] /path/to/input.es " \ + "/path/to/calibration.txt /path/to/divergence.txt " \ + "/path/to/timestamps.txt", \ + "Available options:", \ + " -s s, --spatial-window s sets the radius of the spatial " \ + "window", \ + " defaults to 3", \ + " -t t, --time-decay-first t sets the initial time decay", \ + " defaults to 10000", \ + " -e e, --weight-threshold e sets the weight threshold", \ + " defaults to 0.01", \ + " -m m, --motion-prior m sets the motion uncertainty prior", \ + " defaults to 1000", \ + " -l l, --lambda-prior l sets the parameterized inverse " \ + "depth uncertainty prior", \ + " defaults to 1", \ + " -w w, --width-sampling w sets the width sampling factor", \ + " defaults to 1 (no sampling)", \ + " -g g, --height-sampling g sets the height sampling factor", \ + " defaults to 1 (no sampling)", \ + " -h, --help shows this help message"}, \ + argc, argv, 4, \ + {{"spatial-window", {"s"}}, \ + {"time-decay-first", {"t"}}, \ + {"weight-threshold", {"e"}}, \ + {"motion-prior", {"m"}}, \ + {"lambda-prior", {"l"}}, \ + {"width-sampling", {"w"}}, \ + {"height-sampling", {"g"}}}, \ + {}, [&](pontella::command command) { \ + const std::string& filename = command.arguments[0]; \ + const auto header = \ + sepia::read_header(sepia::filename_to_ifstream(filename)); \ + const std::string& calibration_filename = command.arguments[1]; \ + const std::string& divergence_filename = command.arguments[2]; \ + const std::string& timestamps_filename = command.arguments[3]; \ + \ + Arguments arguments; \ + arguments.spatial_window = \ + extract_argument(command, "spatial-window", 3); \ + arguments.t_decay_first = \ + extract_argument(command, "time-decay-first", 10000); \ + arguments.weight_thresh = \ + extract_argument(command, "weight-threshold", 0.01); \ + arguments.motion_vars_prior = \ + extract_argument(command, "motion-prior", 1000.0); \ + arguments.lambda_prior = \ + extract_argument(command, "lambda-prior", 1.0); \ + arguments.width_sampling_factor = \ + extract_argument(command, "width-sampling", 1.0); \ + arguments.height_sampling_factor = \ + extract_argument(command, "height-sampling", 1.0); \ + \ + auto distorted_to_undistorted = [](LeakyEvent event, float x, \ + float y) -> RectifiedEvent { \ + return {event.t, event.x, event.y, x, y, event.is_increase}; \ + }; \ + \ + auto rectify = make_rectify(calibration_filename, 0, \ + arguments.width_sampling_factor, \ + arguments.height_sampling_factor); \ + \ + Decay event_decay; \ + auto handle_global_decay = [&](Decay decay) { \ + event_decay = decay; \ + }; \ + \ + auto global_decay = make_global_decay( \ + arguments.t_decay_first, \ + [](RectifiedEvent event, float decay, float n_decay, \ + float t_decay, float rate) -> Decay { \ + return {event.t, decay, n_decay, t_decay, rate}; \ + }, \ + handle_global_decay); \ + \ + float ree = 0; \ + uint64_t number_measurements = 0; \ + uint64_t t = 0; \ + float divergence_gt = 0; \ + float divergence_num = 0; \ + float divergence_den = 0; \ + std::ifstream divergence_fin( \ + filename_to_ifstream(divergence_filename)); \ + std::ifstream timestamps_fin( \ + filename_to_ifstream(timestamps_filename)); \ + auto handle_global_motion = [&](GlobalMotionEvent motion) { \ + if (motion.inverse_depth > 0) \ + { \ + divergence_num = event_decay.decay * divergence_num - \ + motion.motion_vars(MotionModel::NV - 1) * \ + motion.inverse_depth; \ + divergence_den = \ + event_decay.decay * divergence_den + static_cast(1); \ + \ + if (t == 0) \ + { \ + read_value(divergence_fin, divergence_gt); \ + read_value(timestamps_fin, t); \ + } \ + else if (t <= motion.t) \ + { \ + const float divergence = divergence_num / divergence_den; \ + ree += std::abs(divergence - divergence_gt) / divergence_gt; \ + ++number_measurements; \ + \ + read_value(divergence_fin, divergence_gt); \ + read_value(timestamps_fin, t); \ + } \ + } \ + }; \ + \ + auto global_motion = \ + make_global_motion_inverse_depth( \ + arguments.spatial_window, arguments.weight_thresh, \ + arguments.motion_vars_prior, arguments.lambda_prior, \ + event_decay, rectify, \ + [](RectifiedEvent event, \ + Vector motion_vars, \ + Vector v, \ + float inverse_depth) -> GlobalMotionEvent { \ + return {event.t, \ + event.x_original, \ + event.y_original, \ + event.x, \ + event.y, \ + motion_vars, \ + v(0), \ + v(1), \ + inverse_depth, \ + event.is_increase}; \ + }, \ + handle_global_motion); \ + \ + auto event_to_leaky_event = [](Event event, uint16_t x, \ + uint16_t y) -> LeakyEvent { \ + return {event.t, event.x, event.y, x, y, event.is_increase}; \ + }; \ + \ + auto leaky_sampling = make_leaky_sampling( \ + header.width, header.height, arguments.width_sampling_factor, \ + arguments.height_sampling_factor, event_to_leaky_event, \ + [&](LeakyEvent event) { \ + const CvVector& p = rectify.map(event.x, event.y); \ + \ + const short x = static_cast(std::round(p[0])); \ + const short y = static_cast(std::round(p[1])); \ + if (0 <= x && x < rectify.width() && 0 <= y && \ + y < rectify.height()) \ + { \ + const RectifiedEvent event_rectified( \ + distorted_to_undistorted(event, p[0], p[1])); \ + \ + global_decay(event_rectified); \ + global_motion(event_rectified); \ + } \ + }); \ + \ + sepia::join_observable(sepia::filename_to_ifstream(filename), \ + leaky_sampling); \ + \ + ree *= static_cast(100) / number_measurements; \ + std::cout << "REE: " << ree << " (%)\n"; \ + }); \ + } + +#endif // ETTCM_GLOBAL_DIVERGENCE_HPP diff --git a/src/global_divergence_scaling.cpp b/src/global_divergence_scaling.cpp new file mode 100644 index 0000000..c51eea0 --- /dev/null +++ b/src/global_divergence_scaling.cpp @@ -0,0 +1,3 @@ +#include "global_divergence.hpp" + +GLOBAL_DIVERGENCE(Scaling) diff --git a/src/global_divergence_six_dof.cpp b/src/global_divergence_six_dof.cpp new file mode 100644 index 0000000..07beb7b --- /dev/null +++ b/src/global_divergence_six_dof.cpp @@ -0,0 +1,3 @@ +#include "global_divergence.hpp" + +GLOBAL_DIVERGENCE(ParameterizedSixDOF) diff --git a/src/global_divergence_translation.cpp b/src/global_divergence_translation.cpp new file mode 100644 index 0000000..2a5e668 --- /dev/null +++ b/src/global_divergence_translation.cpp @@ -0,0 +1,3 @@ +#include "global_divergence.hpp" + +GLOBAL_DIVERGENCE(Translation3D) diff --git a/table_2_partial_results.sh b/table_2_partial_results.sh new file mode 100644 index 0000000..298d0bf --- /dev/null +++ b/table_2_partial_results.sh @@ -0,0 +1,28 @@ +#!/bin/bash + +datapath=./datasets/VL +executablepath=./build/src +calib=$datapath/calib.txt + +for model in scaling translation six_dof +do + echo "Model: $model" + executable=$executablepath/global_divergence_$model + + for s in 2 3 + do + echo " Neighborhood size: $s" + + for seq in 2D-1 2D-3 2D-5 2D-7 3D + do + echo " Sequence: $seq" + + pathseq=$datapath/$seq + events=$pathseq/events.es + divergence=$pathseq/divergence.txt + timestamps=$pathseq/timestamps.txt + + $executable $events $calib $divergence $timestamps -s $s -e 0.01 -m 1000 -l 1 + done + done +done diff --git a/table_2_results.sh b/table_2_results.sh new file mode 100644 index 0000000..f6e3140 --- /dev/null +++ b/table_2_results.sh @@ -0,0 +1,28 @@ +#!/bin/bash + +datapath=./datasets/VL +executablepath=./build/src +calib=$datapath/calib.txt + +for model in scaling translation six_dof +do + echo "Model: $model" + executable=$executablepath/global_divergence_$model + + for s in 2 3 + do + echo " Neighborhood size: $s" + + for seq in 2D-1 2D-2 2D-3 2D-4 2D-5 2D-6 2D-7 3D + do + echo " Sequence: $seq" + + pathseq=$datapath/$seq + events=$pathseq/events.es + divergence=$pathseq/divergence.txt + timestamps=$pathseq/timestamps.txt + + $executable $events $calib $divergence $timestamps -s $s -e 0.01 -m 1000 -l 1 + done + done +done diff --git a/table_6_partial_results.sh b/table_6_partial_results.sh new file mode 100644 index 0000000..443e00d --- /dev/null +++ b/table_6_partial_results.sh @@ -0,0 +1,33 @@ +#!/bin/bash + +datapath=./datasets/VL +executablepath=./build/src +calib=$datapath/calib.txt + +for model in scaling translation six_dof +do + echo "Model: $model" + executable=$executablepath/global_divergence_$model + + for s in 2 3 + do + echo " Neighborhood size: $s" + + for r in 2 3 4 + do + echo " Resolution reduction multiplier: $r" + + for seq in 2D-1 2D-3 2D-5 2D-7 3D + do + echo " Sequence: $seq" + + pathseq=$datapath/$seq + events=$pathseq/events.es + divergence=$pathseq/divergence.txt + timestamps=$pathseq/timestamps.txt + + $executable $events $calib $divergence $timestamps -s $s -e 0.01 -m 1000 -l 1 -w $r -g $r + done + done + done +done diff --git a/table_6_results.sh b/table_6_results.sh new file mode 100644 index 0000000..efbd3d1 --- /dev/null +++ b/table_6_results.sh @@ -0,0 +1,33 @@ +#!/bin/bash + +datapath=./datasets/VL +executablepath=./build/src +calib=$datapath/calib.txt + +for model in scaling translation six_dof +do + echo "Model: $model" + executable=$executablepath/global_divergence_$model + + for s in 2 3 + do + echo " Neighborhood size: $s" + + for r in 2 3 4 + do + echo " Resolution reduction multiplier: $r" + + for seq in 2D-1 2D-2 2D-3 2D-4 2D-5 2D-6 2D-7 3D + do + echo " Sequence: $seq" + + pathseq=$datapath/$seq + events=$pathseq/events.es + divergence=$pathseq/divergence.txt + timestamps=$pathseq/timestamps.txt + + $executable $events $calib $divergence $timestamps -s $s -e 0.01 -m 1000 -l 1 -w $r -g $r + done + done + done +done