diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..e4a8599
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,4 @@
+output/*
+matlab/paper_data/*
+*.mat
+*.m~
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 05609e1..88b1573 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,10 +1,22 @@
-cmake_minimum_required (VERSION 2.8.3)
-project (lidar_tag)
+cmake_minimum_required (VERSION 3.1)
+project (lidartag)
# CMAKE TWEAKS
#========================================================================
-set(CMAKE_CXX_STANDARD 11)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3 -DEIGEN_NO_DEBUG -march=native -Wl,--no-as-needed")
+# set(CMAKE_CXX_STANDARD 14)
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -O3 -DEIGEN_NO_DEBUG -march=native -Wl,--no-as-needed")
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -O3 -DEIGEN_NO_DEBUG -march=native -Wl,--no-as-needed")
+# set(MATLAB_INCLUDE_DIRS /usr/local/MATLAB/R2019b/extern/include/)
+# set(MATLAB_LIBRARIES_DIRS /usr/local/MATLAB/R2019b/bin/glnxa64)
+# set(MATLAB_LIBRARIES ${MATLAB_LIBRARIES_DIRS}/libmat.so ${MATLAB_LIBRARIES_DIRS}/libmx.so)
+# list(APPEND CMAKE_PREFIX_PATH "/home/brucebot/softwares/oneTBB")
+# list(APPEND CMAKE_PREFIX_PATH "/usr/local/include")
+# list(APPEND CMAKE_PREFIX_PATH "/usr/local/lib/cmake")
+# list(APPEND CMAKE_PREFIX_PATH "/home/brucebot/softwares/tbb_outsourced/build2")
+# set(CMAKE_MODULE_PATH "/home/brucebot/softwares/TBB/build/;/usr/local/lib/;/usr/local/lib/cmake/;${CMAKE_MODULE_PATH}")
+LIST(APPEND CMAKE_MODULE_PATH "/home/brucebot/workspace/catkin/src/LiDARTag/cmake/")
+message(STATUS "=============================================CMAKE_MODULE_PATH: ${CMAKE_MODULE_PATH}")
find_package(catkin REQUIRED COMPONENTS
roscpp
@@ -19,33 +31,60 @@ find_package(catkin REQUIRED COMPONENTS
lidartag_msgs
)
# CHECK THE DEPENDENCIES
+find_package(NLopt REQUIRED)
# PCL
find_package(PCL 1.2 REQUIRED)
find_package(Boost REQUIRED
COMPONENTS filesystem system signals regex date_time program_options thread
)
+# find_package(Eigen3)
+#include(cmake)
+# include(${CMAKE_CURRENT_SOURCE_DIR}/../A/CMakeLists.txt)
+# find_package(Eigen3 HINTS /home/brucebot/softwares/eigen_git/cmake/)
+# set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
+# message(STATUS "=============================================Eigen path: ${EIGEN3_INCLUDE_DIR}")
+# set(EIGEN3_INCLUDE_DIR "/usr/local/include/eigen3/")
+
+message(STATUS "===============================NLOPT lib: ${NLOPT_LIBRARIES}")
+find_package(TBB REQUIRED)
+if(TBB_FOUND)
+ message(STATUS "=============================================TBB FOUND")
+ message(STATUS "===============================TBB include_dirs: ${TBB_INCLUDE_DIRS}")
+ message(STATUS "===============================TBB includes: ${TBB_INCLUDES}")
+ message(STATUS "===============================TBB libs: ${TBB_LIBS}")
+ message(STATUS "===============================TBB libraries: ${TBB_LIBRARIES}")
+ message(STATUS "===============================TBB libraries: ${TBB_LIBRARIES}")
+ message(STATUS "===============================TBB libs: ${TBB_LIBS}")
+ message(STATUS "===============================TBB defs: ${TBB_DEFINITIONS}")
+else()
+ message(STATUS "=============================================TBB NOT FOUND")
+endif()
+
+
find_package(Eigen3)
if(NOT EIGEN3_FOUND)
# Fallback to cmake_modules
find_package(cmake_modules REQUIRED)
+ # message(STATUS "=============================================Eigen path: ${EIGEN3_INCLUDE_DIR}")
find_package(Eigen REQUIRED)
set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) # Not strictly necessary as Eigen is head only
# Possibly map additional variables to the EIGEN3_ prefix.
else()
+ # message(STATUS "=============================================Eigen path: ${EIGEN3_INCLUDE_DIR}")
set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
endif()
+find_package(Qt5 COMPONENTS Core Widgets REQUIRED)
catkin_package(
- INCLUDE_DIRS include
- CATKIN_DEPENDS
- message_filters
+ INCLUDE_DIRS include
+ CATKIN_DEPENDS
+ message_filters
roscpp
sensor_msgs
- std_msgs
+ std_msgs
tf
-
- DEPENDS
+ DEPENDS
Eigen3
)
@@ -53,19 +92,23 @@ include_directories(
include
${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
- ${Boost_INCLUDE_DIRS}
- ${EIGEN3_INCLUDE_DIRS}
+ ${Boost_INCLUDE_DIRS}
+ ${EIGEN3_INCLUDE_DIRS}
+ ${TBB_INCLUDE_DIRS}
)
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
# COMPILE THE SOURCE
#========================================================================
-add_executable(lidar_tag_main src/main.cc src/lidar_tag.cc src/apriltag_utils.cc src/utils.cc src/tag49h14.cc src/tag16h5.cc)
-add_dependencies(lidar_tag_main ${${PROJECT_NAME}_EXPORTED_TARGETS}
+add_executable(lidartag_main src/main.cc src/lidartag_pose.cc src/lidartag.cc src/lidartag_decode.cc src/apriltag_utils.cc src/utils.cc src/tag49h14.cc src/tag16h5.cc src/lidartag_rviz.cc src/lidartag_cluster.cc src/lidartag_prune.cc)
+
+add_dependencies(lidartag_main ${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS})
-target_link_libraries(lidar_tag_main
+target_link_libraries(lidartag_main
${catkin_LIBRARIES}
${Boost_LIBRARIES}
- ${PCL_LIBRARIES}
+ ${PCL_LIBRARIES}
+ ${NLOPT_LIBRARIES}
+ ${TBB_LIBRARIES}
)
diff --git a/LICENSE b/LICENSE.md
similarity index 86%
rename from LICENSE
rename to LICENSE.md
index f288702..0ad25db 100644
--- a/LICENSE
+++ b/LICENSE.md
@@ -1,5 +1,5 @@
- GNU GENERAL PUBLIC LICENSE
- Version 3, 29 June 2007
+ GNU AFFERO GENERAL PUBLIC LICENSE
+ Version 3, 19 November 2007
Copyright (C) 2007 Free Software Foundation, Inc.
Everyone is permitted to copy and distribute verbatim copies
@@ -7,17 +7,15 @@
Preamble
- The GNU General Public License is a free, copyleft license for
-software and other kinds of works.
+ The GNU Affero General Public License is a free, copyleft license for
+software and other kinds of works, specifically designed to ensure
+cooperation with the community in the case of network server software.
The licenses for most software and other practical works are designed
to take away your freedom to share and change the works. By contrast,
-the GNU General Public License is intended to guarantee your freedom to
+our General Public Licenses are intended to guarantee your freedom to
share and change all versions of a program--to make sure it remains free
-software for all its users. We, the Free Software Foundation, use the
-GNU General Public License for most of our software; it applies also to
-any other work released this way by its authors. You can apply it to
-your programs, too.
+software for all its users.
When we speak of free software, we are referring to freedom, not
price. Our General Public Licenses are designed to make sure that you
@@ -26,44 +24,34 @@ them if you wish), that you receive source code or can get it if you
want it, that you can change the software or use pieces of it in new
free programs, and that you know you can do these things.
- To protect your rights, we need to prevent others from denying you
-these rights or asking you to surrender the rights. Therefore, you have
-certain responsibilities if you distribute copies of the software, or if
-you modify it: responsibilities to respect the freedom of others.
-
- For example, if you distribute copies of such a program, whether
-gratis or for a fee, you must pass on to the recipients the same
-freedoms that you received. You must make sure that they, too, receive
-or can get the source code. And you must show them these terms so they
-know their rights.
-
- Developers that use the GNU GPL protect your rights with two steps:
-(1) assert copyright on the software, and (2) offer you this License
-giving you legal permission to copy, distribute and/or modify it.
-
- For the developers' and authors' protection, the GPL clearly explains
-that there is no warranty for this free software. For both users' and
-authors' sake, the GPL requires that modified versions be marked as
-changed, so that their problems will not be attributed erroneously to
-authors of previous versions.
-
- Some devices are designed to deny users access to install or run
-modified versions of the software inside them, although the manufacturer
-can do so. This is fundamentally incompatible with the aim of
-protecting users' freedom to change the software. The systematic
-pattern of such abuse occurs in the area of products for individuals to
-use, which is precisely where it is most unacceptable. Therefore, we
-have designed this version of the GPL to prohibit the practice for those
-products. If such problems arise substantially in other domains, we
-stand ready to extend this provision to those domains in future versions
-of the GPL, as needed to protect the freedom of users.
-
- Finally, every program is threatened constantly by software patents.
-States should not allow patents to restrict development and use of
-software on general-purpose computers, but in those that do, we wish to
-avoid the special danger that patents applied to a free program could
-make it effectively proprietary. To prevent this, the GPL assures that
-patents cannot be used to render the program non-free.
+ Developers that use our General Public Licenses protect your rights
+with two steps: (1) assert copyright on the software, and (2) offer
+you this License which gives you legal permission to copy, distribute
+and/or modify the software.
+
+ A secondary benefit of defending all users' freedom is that
+improvements made in alternate versions of the program, if they
+receive widespread use, become available for other developers to
+incorporate. Many developers of free software are heartened and
+encouraged by the resulting cooperation. However, in the case of
+software used on network servers, this result may fail to come about.
+The GNU General Public License permits making a modified version and
+letting the public access it on a server without ever releasing its
+source code to the public.
+
+ The GNU Affero General Public License is designed specifically to
+ensure that, in such cases, the modified source code becomes available
+to the community. It requires the operator of a network server to
+provide the source code of the modified version running there to the
+users of that server. Therefore, public use of a modified version, on
+a publicly accessible server, gives the public access to the source
+code of the modified version.
+
+ An older license, called the Affero General Public License and
+published by Affero, was designed to accomplish similar goals. This is
+a different license, not a version of the Affero GPL, but Affero has
+released a new version of the Affero GPL which permits relicensing under
+this license.
The precise terms and conditions for copying, distribution and
modification follow.
@@ -72,7 +60,7 @@ modification follow.
0. Definitions.
- "This License" refers to version 3 of the GNU General Public License.
+ "This License" refers to version 3 of the GNU Affero General Public License.
"Copyright" also means copyright-like laws that apply to other kinds of
works, such as semiconductor masks.
@@ -549,35 +537,45 @@ to collect a royalty for further conveying from those to whom you convey
the Program, the only way you could satisfy both those terms and this
License would be to refrain entirely from conveying the Program.
- 13. Use with the GNU Affero General Public License.
+ 13. Remote Network Interaction; Use with the GNU General Public License.
+
+ Notwithstanding any other provision of this License, if you modify the
+Program, your modified version must prominently offer all users
+interacting with it remotely through a computer network (if your version
+supports such interaction) an opportunity to receive the Corresponding
+Source of your version by providing access to the Corresponding Source
+from a network server at no charge, through some standard or customary
+means of facilitating copying of software. This Corresponding Source
+shall include the Corresponding Source for any work covered by version 3
+of the GNU General Public License that is incorporated pursuant to the
+following paragraph.
Notwithstanding any other provision of this License, you have
permission to link or combine any covered work with a work licensed
-under version 3 of the GNU Affero General Public License into a single
+under version 3 of the GNU General Public License into a single
combined work, and to convey the resulting work. The terms of this
License will continue to apply to the part which is the covered work,
-but the special requirements of the GNU Affero General Public License,
-section 13, concerning interaction through a network will apply to the
-combination as such.
+but the work with which it is combined will remain governed by version
+3 of the GNU General Public License.
14. Revised Versions of this License.
The Free Software Foundation may publish revised and/or new versions of
-the GNU General Public License from time to time. Such new versions will
-be similar in spirit to the present version, but may differ in detail to
+the GNU Affero General Public License from time to time. Such new versions
+will be similar in spirit to the present version, but may differ in detail to
address new problems or concerns.
Each version is given a distinguishing version number. If the
-Program specifies that a certain numbered version of the GNU General
+Program specifies that a certain numbered version of the GNU Affero General
Public License "or any later version" applies to it, you have the
option of following the terms and conditions either of that numbered
version or of any later version published by the Free Software
Foundation. If the Program does not specify a version number of the
-GNU General Public License, you may choose any version ever published
+GNU Affero General Public License, you may choose any version ever published
by the Free Software Foundation.
If the Program specifies that a proxy can decide which future
-versions of the GNU General Public License can be used, that proxy's
+versions of the GNU Affero General Public License can be used, that proxy's
public statement of acceptance of a version permanently authorizes you
to choose that version for the Program.
@@ -635,40 +633,29 @@ the "copyright" line and a pointer to where the full notice is found.
Copyright (C)
This program is free software: you can redistribute it and/or modify
- it under the terms of the GNU General Public License as published by
- the Free Software Foundation, either version 3 of the License, or
+ it under the terms of the GNU Affero General Public License as published
+ by the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- GNU General Public License for more details.
+ GNU Affero General Public License for more details.
- You should have received a copy of the GNU General Public License
+ You should have received a copy of the GNU Affero General Public License
along with this program. If not, see .
Also add information on how to contact you by electronic and paper mail.
- If the program does terminal interaction, make it output a short
-notice like this when it starts in an interactive mode:
-
- Copyright (C)
- This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
- This is free software, and you are welcome to redistribute it
- under certain conditions; type `show c' for details.
-
-The hypothetical commands `show w' and `show c' should show the appropriate
-parts of the General Public License. Of course, your program's commands
-might be different; for a GUI interface, you would use an "about box".
+ If your software can interact with users remotely through a computer
+network, you should also make sure that it provides a way for users to
+get its source. For example, if your program is a web application, its
+interface could display a "Source" link that leads users to an archive
+of the code. There are many ways you could offer source, and different
+solutions will be better for different programs; see section 13 for the
+specific requirements.
You should also get your employer (if you work as a programmer) or school,
if any, to sign a "copyright disclaimer" for the program, if necessary.
-For more information on this, and how to apply and follow the GNU GPL, see
+For more information on this, and how to apply and follow the GNU AGPL, see
.
-
- The GNU General Public License does not permit incorporating your program
-into proprietary programs. If your program is a subroutine library, you
-may consider it more useful to permit linking proprietary applications with
-the library. If this is what you want to do, use the GNU Lesser General
-Public License instead of this License. But first, please read
-.
diff --git a/LiDARTag.pdf b/LiDARTag.pdf
new file mode 100644
index 0000000..9da27a4
Binary files /dev/null and b/LiDARTag.pdf differ
diff --git a/README.md b/README.md
index 5cf59ac..52d5dc0 100644
--- a/README.md
+++ b/README.md
@@ -1,42 +1,353 @@
# LiDARTag
+
## Overview
+This is a package for LiDARTag, described in paper: **LiDARTag: A Real-Time Fiducial Tag System for Point Clouds** ([PDF](./LiDARTag.pdf))([arXiv](https://arxiv.org/abs/1908.10349)). This work is accepted by IEEE Robotics and Automation Letters and published at ([here](https://ieeexplore.ieee.org/abstract/document/9392337)).
+
-This is a ROS package for detecting the LiDARTag via ROS.
+Image-based fiducial markers are useful in problems such as object tracking in cluttered or textureless environments, camera (and multi-sensor) calibration tasks, and vision-based simultaneous localization and mapping (SLAM). However, the state-of-the-art fiducial marker detection algorithms rely on the consistency of the ambient lighting. To the best of our knowledge, there are no existing fiducial markers for point clouds.
-* Author: Bruce JK Huang
+This paper introduces LiDARTag, a novel fiducial tag design and detection algorithm suitable for LiDAR point clouds. The proposed method runs in real-time and can process data at 100 Hz, which is faster than the currently available LiDAR sensor frequencies. Additionally, the software works with different marker sizes in cluttered indoors and spacious outdoors, even when it is entirely dark. Because of the LiDAR sensors' nature, rapidly changing ambient lighting will not affect the detection of a LiDARTag. Therefore, LiDARTag can be used in tandem with camera-based markers to address the issue of images being sensitive to ambient lighting.
+
+* Author: Jiunn-Kai (Bruce) Huang, Shoutian Wang, Maani Ghaffari, and Jessy W. Grizzle
* Maintainer: [Bruce JK Huang](https://www.brucerobot.com/), brucejkh[at]gmail.com
* Affiliation: [The Biped Lab](https://www.biped.solutions/), the University of Michigan
-This package has been tested under [ROS] Kinetic and Ubuntu 16.04.
-More detail introduction will be update shortly. Sorry for the inconvenient!
+This package has been tested under [ROS] Melodic and Ubuntu 18.04.\
+**[Note]** More detailed introduction will be updated shortly. Sorry for the inconvenient!\
+**[Issues]** If you encounter _any_ issues, I would be happy to help. If you cannot find a related one in the existing issues, please open a new one. I will try my best to help!
+
+
+## Abstract
+Image-based fiducial markers are useful in problems such as object tracking in cluttered or textureless environments, camera (and multi-sensor) calibration tasks, and vision-based simultaneous localization and mapping (SLAM). The state-of-the-art fiducial marker detection algorithms rely on the consistency of the ambient lighting. This paper introduces LiDARTag, a novel fiducial tag design and detection algorithm suitable for light detection and ranging (LiDAR) point clouds. The proposed method runs in real-time and can process data at 100 Hz, which is faster than the currently available LiDAR sensor frequencies. Because of the LiDAR sensors' nature, rapidly changing ambient lighting will not affect the detection of a LiDARTag; hence, the proposed fiducial marker can operate in a completely dark environment. In addition, the LiDARTag nicely complements and is compatible with existing visual fiducial markers, such as AprilTags, allowing for efficient multi-sensor fusion and calibration tasks. We further propose a concept of minimizing a fitting error between a point cloud and the marker's template to estimate the marker's pose. The proposed method achieves millimeter error in translation and a few degrees in rotation. Due to LiDAR returns' sparsity, the point cloud is lifted to a continuous function in a reproducing kernel Hilbert space where the inner product can be used to determine a marker's ID. The experimental results, verified by a motion capture system, confirm that the proposed method can reliably provide a tag's pose and unique ID code. The rejection of false positives is validated on the [Google Cartographer](https://google-cartographer-ros.readthedocs.io/en/latest/data.html) dataset and the outdoor [Honda H3D](https://usa.honda-ri.com/H3D) datasets. All implementations are coded in C++ and are available at: [https://github.com/UMich-BipedLab/LiDARTag](https://github.com/UMich-BipedLab/LiDARTag).
+
+
+## Video
+Please checkout the introduction [video](https://www.brucerobot.com/lidartag). It highlights some important keypoints in the paper!
+[
](https://www.brucerobot.com/lidartag)
+
+
## Quick View
-
-
+LiDAR-based markers can be used in tandem with camera-based markers to address the issue of images being sensitive to ambient lighting. LiDARTags have been successfully applied to LiDAR-camera extrinsic calibration ([paper](https://ieeexplore.ieee.org/document/9145571), [GitHub](https://github.com/UMich-BipedLab/extrinsic_lidar_camera_calibration)). This figure shows a visualization of LiDARTags of two different sizes in a full point cloud scan.
+
+This system runs in real-time (over 100 Hz) while handling a full scan of the point cloud; it achieves millimeter accuracy in translation and a few degrees of error in rotation. The tag decoding accuracy is 99.7%.
+
+
+
+
+## Why LiDAR?
+Robust to lighting!! The following shows LiDARTags are detected in several challenging lighting conditions:
+##### Dingy environment
+
+
+##### Completely dark environment
+
+
+##### Half tag being overexposed
+
+
+##### Rapid changing ambient light
+
+
+## Overall pipeline
+The system contains three parts: tag detection, pose estimation, and tag decoding. The detection step takes an entire LiDAR scan (up to 120,000 points from a 32-Beam Velodyne ULTRA Puck LiDAR) and outputs collections of likely payload points of the LiDARTag. Next, a tag's optimal pose minimizes the
-inspired cost in (8), though the rotation of the tag about a normal vector to the tag may be off by
or
and will be resolved in the decoding process. The tag's ID is decoded with a pre-computed function library. The decoded tag removes the rotation ambiguity about the normal.
+
+
+
+
+## Package Analysis
+We present performance evaluations of the LiDARTag where ground truth data are provided by a motion capture system with 30 motion capture cameras. We also extensively analyze each step in the system with spacious outdoor and cluttered indoor environments. Additionally, we report the rate of false positives validated on the indoor [Google Cartographer](https://google-cartographer-ros.readthedocs.io/en/latest/data.html) dataset and the outdoor [Honda H3D](https://usa.honda-ri.com/H3D) datasets.
+
+#### Pose and Decoding Analysis
+Decoding accuracy of the RKHS method and pose accuracy of the fitting method. The ground truth is provided by a motion capture system with 30 motion capture cameras. The distance is in meters. The translation error is in millimeters and rotation error is the misalignment angle, (23), in degrees.
+
+
+#### Computation Time of Each Step Analysis
+This table averages all the datasets we collected and describes computation time of each step for indoors and outdoors.
+
+
+
+### Cluster Rejection Analysis
+This table takes into account all the data we collected and shows numbers of rejected clusters in each step in different scenes. Additionally, we also report false positive rejection for [Google Cartographer](https://google-cartographer-ros.readthedocs.io/en/latest/data.html) dataset and [Honda H3D](https://usa.honda-ri.com/H3D) datasets.
+
+
+#### Double-Sum Analysis
+The original double sum in (18) is too slow to achieve a real-time application. This table compares different methods to compute the double sum, in which the TBB stands for Threading Building Blocks library from Intel. Additionally, we also apply a k-d tree data structure to speed up the querying process; the k-d tree, however, does not produce fast enough results. The unit in the table is milliseconds.
+
+
+#### False Positives Analysis
+This table shows the numbers of false positive rejection of the proposed algorithm. We validated the rejection rate on the indoor [Google Cartographer](https://google-cartographer-ros.readthedocs.io/en/latest/data.html) dataset and the outdoor [Honda H3D](https://usa.honda-ri.com/H3D) datasets. The former has two VLP-16 Velodyne LiDAR and the latter has one 64-beam Velodyne LiDAR.
+
+
+## Required Libraries / Packages
+Those are the packages used in the LiDARTag package. It seems many but if you follow my steps, it should take you no more than 30 mins to instal them (including building time!). It took me awhile to get everything right. I summarize how I installed them [here](#installation-of-related-libraries). However, you may choose any way you want to install them.
+1. Please install [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu).
+2. Please install [TBB library](https://github.com/oneapi-src/oneTBB). You may need to modify the CMakeLists.txt according to your installation.
+3. Please install [NLopt](https://nlopt.readthedocs.io/en/latest/). You may need to midify the CMakeLists.txt according to your installation.
+4. Please download [LiDARTag_msgs](https://github.com/UMich-BipedLab/LiDARTag_msgs) and place them under your catkin workspace.
+5. Plesae ensure you have a correct Eigen Library on your system by downloading it from the [GitLab](https://gitlab.com/libeigen/eigen.git) and checkout `6f0f6f792e441c32727ed945686fefe02e6bdbc6`. Any commit older than this should also work.
+
+## Installation of Related Libraries
+### ROS Melodic
+Please directly follow the instruction on the official website ([here](http://wiki.ros.org/melodic/Installation/Ubuntu)).
+
+### TBB library
+###### Installation
+Original TBB package from Intel does not support CMake; I, therefore, use another repository that supports CMake to make my life easier.
+```
+git clone https://github.com/wjakob/tbb
+mkdir build;
+cd build;
+cmake ..;
+cmake --build . --config Release -- -j 6;
+sudo cmake --build . --target install
+```
+
+###### Notes
+Ensure the followings in the CMakeList.txt are correct:
+1. FindTBB.cmake is under _LiDARTag/cmake/_
+2. LIST(APPEND CMAKE_MODULE_PATH "YOUR_PATH/LiDARTag/cmake/")
+ * Please change **YOUR_PATH** to your path to the LiDARTag package (something like this: catkin/src/LiDARTag/cmake).
+
+
+### NLopt library
+###### Installation
+Please direct follow the instruction on the official website ([here](https://nlopt.readthedocs.io/en/latest/NLopt_Installation/)) or as follow:
+```
+git clone git://github.com/stevengj/nlopt
+cd nlopt
+mkdir build
+cd build
+cmake ..
+make
+sudo make install
+```
+
+### LiDARTag package
+1. Once you place LiDARTag_msgs under your catkin workspace and installed all the required libraries, you can directly `catkin_make` the package.
+2. `source devel/setup.bash`
+3. `roslaunch lidartag LiDARTag_twotags.launch`
+4. `rosbag play -l -q bagfile.bag`
+
+
+
+## Datasets and Results
+### Quantitative results:
+If you would like to see how the tables in the paper are generated, please follow as below:
+1. Download [this folder](https://drive.google.com/drive/folders/16bJx_Hc7iqy5-rxjWVgXOhvJGULu4hI9?usp=sharing).
+2. Put them under _LiDARTag/matlab/paper_data/_
+3. Run _genTable.m_ located at _LiDARTag/matlab/_
+
+
+To regenerate results on the paper from scratch, please download the two datasets below:
+1. Please download bagfiles from [here](https://drive.google.com/drive/folders/1k-p3q2hl1ZRIcle1lfcS0AN-DAK91RMw?usp=sharing).
+2. Please download motion capture data from [here](https://drive.google.com/drive/folders/1rWCRsktFPZiOyQopwG7vCXatqwU-EyFb?usp=sharing)
+3. change the _output_path_ in the launch file
+4. `roslaunch lidartag LiDARTag_threetags.launch`
+
+###### Note
+The target sizes in the quantitative result folder are 1.22.
+
+### Qualitative results:
+1. Please download bagfiles from [here](https://drive.google.com/drive/folders/19w13uJNMax9_fNIzmFZwzYq2LpJsjKKh?usp=sharing).
+2. `roslaunch lidartag LiDARTag_twotags.launch`
+
+###### Note
+The target sizes in the qualitative result folder are 0.8051, 0.61.
+
+
+### False positive rejection:
+Please download [Google Cartographer](https://google-cartographer-ros.readthedocs.io/en/latest/data.html) dataset and [Honda H3D](https://usa.honda-ri.com/H3D) datasets.
+We also provide different launch files (cartographer.launch, H3D.launch) for different datasets due to different published LiDAR topics and different _output_path_. I also wrote my own parsing script to pass bin files to rosbag. Please let me know if anyone needs it.
+
+
+## Running
+1. Please download qualitative bagfiles from [here](https://drive.google.com/drive/folders/19w13uJNMax9_fNIzmFZwzYq2LpJsjKKh?usp=sharing).
+2. `catkin_make` the package.
+3. `source devel/setup.bash`
+4. `roslaunch lidartag LiDARTag_twotags.launch`
+5. `rosbag play -l -q bagfile.bag`
+6. To see the results, `rosrun rviz rviz`. You can directly open _LiDARTag.rviz_ under _LiDARTag/rviz/_ folder.
+
+
+###### Notes
+This package provides several launch files that you can directly run the package.\
+Please remember to change the **tag_size_list** in a launch file according to your target sizes or which bag file you are playing, or what marker sizes you have.\
+Different launch files:\
+-- LiDARTag_smallest.launch: only the smallest tag (0.61)\
+-- LiDARTag_twotags.launch: two smaller tags (0.61, 0.8)\
+-- LiDARTag_threetags.launch: all tags (0.8, 0.61, 1.22)\
+Please note that, the clearance around the markers should larger than
, where
is the size of the **largest** marker. Therefore, it is recommended to use smaller tags in indoor environments.
+
+
+
+## Building Your Markers
+
\
+We provide tag16h6c5 from AprilTag3 with three sizes (0.61, 0.85, 1.2).\
+If you want to use the provided markers, it is easy:
+1. Attach a fiducial marker to a squared cardboard or plexiglass and place the marker inside the yellow region.
+ * Note: The sizes **must be** one of 0.61, 0.805, 1.22 meter, or you **have to** regenerate the function dictionary. If so, please follow [here](#building-your-own-customized-markers).
+2. Find a 3D object to _support_ your marker. It could be a box or an easel.
+ * Please note that, the clearance around the markers should larger than
, where
is the size of the **largest** marker. Therefore, it is recommended to use smaller tags in indoor environments.
+4. Follow [these steps](#running) to run the package.
+
+## Building Your Own Customized Markers
+If you would like to use your own customized markers (i.e. different types of markers or different sizes), please follow these steps:\
+I. Build your function dictionary:
+1. `git clone https://github.com/UMich-BipedLab/matlab_utils`
+2. Add _matlab_utils_ into _build_LiDARTag_library.m_ or add _matlab_utils_ into your MATLAB path.
+3. Edit **opts.img_path** in _build_LiDARTag_library.m_ according to where you put images of your fiducial markers.
+4. Measure the size of your marker (
)
+5. Open _build_LiDARTag_library.m_ in _LiDARTag/matlab/function_dictionary/_. Change `opts.target_size_` to your marker size and run _build_LiDARTag_library.m_ to generate your function library.
+6. Put the generated function dictuionary into _LiDARTag/lib/_
+7. When placing the generated function dictionary in _LiDARTag/lib/_, please put different sizes into different sub-folders (0, 1, 2, 3, ...) and put them in ascending order. For example, if you have three sizes (0.6, 0.8, 1.2), then you will have three sub-folders (0, 1, 2) inside the _lib/_ folder. Please place them as follow:
+ - LiDARTag/lib/0/: put 0.6-size here
+ - LiDARTag/lib/1/: put 0.8-size here
+ - LiDARTag/lib/2/: put 1.2-size here
+
+II. Follow [Building Your Markers](#building-your-markers)
+###### Note
+All the functions that are used for testing RKHS are all released in _LiDARTag/matlab/function_dictionary/_
+
+
+## Parameters of launch files
+We split the parameters to two different launch files: `LiDARTag_outdoor.launch` and
+`LiDARTag_master.launch.` The front contains the most common tunables for different
+environments such as indoor or outdoor. The latter includes more parameters that you
+usually need to change for your system only once and just leave them there.
+### LiDARTag_outdoor.launch
+##### feature clustering
+* nearby_factor
+ Value used to determine if two points are near to each other
+* linkage_tunable
+ Value used to compute the linkage criteria
+
+##### cluster validation
+* max_outlier_ratio
+ Value used to validate clusters during checking outliers in plane fitting
+* tag_size_list
+ List of possible sizes of tag
+
+### LiDARTag_master.launch
+#### System Mode
+* mark_cluster_validity
+ whether to validate clusters according to different conditions
+* plane_fitting
+ whether to validate clusters according to the result of plane_fitting
+* optimize_pose
+ Whether to optimize poses via reducing the cost function
+* decode_id
+ Whether to decode IDs
+* collect_data
+ Whether to publish detected PCs
+* num_threads
+ The number of threads used for TBB
+* print_info
+ Whether to log status in ros_info_stream
+
+#### Debugging Mode
+* debug_info
+ Whether to log debug information in ros_debug_stream
+* debug_time
+ Whether to compute time for different parts
+* debug_decoding_time
+ Whether to log time for decoding IDs
+* log_data
+ Whether to save status information into txt file
+
+#### LiDAR Specification
+* has_ring
+ Whether input data has ring information for each point
+* estimate_ring
+ Whether to estimate ring number for each point
-# Why LiDAR?
-Robust to lighting!!
-
-
-
+#### Solvers for Pose Optimization
+* optimization_solver (default: 8)
+ Which optimization solver to use for optimizing the cost function of a pose.
+ * Below is numerical gradient-based methods
+ 1: opt_method = nlopt::LN_PRAXIS;
+ 2: opt_method = nlopt::LN_NEWUOA_BOUND;
+ 3: opt_method = nlopt::LN_SBPLX; // recommended
+ 4: opt_method = nlopt::LN_BOBYQA;
+ 5: opt_method = nlopt::LN_NELDERMEAD;
+ 6: opt_method = nlopt::LN_COBYLA;
+ * Below is analytical gradient-based methods
+ 7: opt_method = nlopt::LD_SLSQP; // recommended 200Hz
+ 8: opt_method = nlopt::LD_MMA; // recommended 120Hz
+ 9: opt_method = nlopt::LD_TNEWTON_PRECOND_RESTART; // fail 90%
+ 10: opt_method = nlopt::LD_TNEWTON_PRECOND; // fail 90%
+ 11: opt_method = nlopt::LD_TNEWTON_RESTART; // fail 80%
+ 12: opt_method = nlopt::LD_TNEWTON; // fail 90%
+ 13: opt_method = nlopt::LD_LBFGS; // fail 90%
+ 14: opt_method = nlopt::LD_VAR1; // fail 90%
+ 15: opt_method = nlopt::LD_VAR2; // fail 90%
+* euler_derivative
+ Whether to use euler derivative or lie group derivative in optimization
+* optimize_up_bound
+ Value used for constraints in optimization
+* optimize_low_bound
+ Value used for constraints in optimization
+#### Decode Method
+* decode_method (default: 2)
+ Which decoding method to use:
+ 0: naive decoder
+ 1: Weighted Gaussian
+ 2: RKHS
+* decode_mode (default: 5)
+ Which mode to use:
+ 0: single thread: original double sum
+ 1: single thread: convert to matrices
+ 2: single thread: convert matrices to vectors
+ 3: c++ thread (works for each point for a thread but not for blobs of points for a thread)
+ 4: Multi-threading: Original double sum using TBB
+ 5: Multi-threading: Vector form using TBB without scheduling
+ 6: Multi-threading: Vector form using TBB with manual scheduling
+ 7: Multi-threading: Vector form using TBB with TBB scheduling
+ 8: Single thread: using KDTree
-## Presentation and Video
-https://www.brucerobot.com/lidartag
+#### Tunable
+##### feature clustering
+* distance_bound
+ Value used to construct a cube and only detect the tag inside this cube
+* depth_bound
+ Value used to detect feature points compared with depth gradients
+* num_points_for_plane_feature
+ number of points used for detection of feature points
-## Installation
-TODO
+##### cluster validation
+* min_return_per_grid
+ Minimum number of points in each grid (below this number, the cluster will be invalid)
+* optimize_percentage
+ Value used to validate the result of pose estimation via checking cost value
+* payload_intensity_threshold
+ Value used to detect boundary points on the cluster via intensity gradient
+* points_threshold_factor
+* distance_to_plane_threshold
+ Value used for plane fitting for a cluster
+* minimum_ring_boundary_points
+ Minimum number of boundary points on each ring in the cluster
+* coa_tunable
+ Value used to validate the result of pose estimation via checking coverage area
+* tagsize_tunable
+ Value used to estimate the size of tag
-## Parameters
-TODO
-## Examples
-TODO
## Citations
-The LiDARTag is described in:
+The detail is described in: **LiDARTag: A Real-Time Fiducial Tag for Point Clouds,** Jiunn-Kai Huang, Shoutian Wang, Maani Ghaffari, and Jessy W. Grizzle. ([PDF](./LiDARTag.pdf)) ([arXiv](https://arxiv.org/abs/1908.10349)) ([here](https://ieeexplore.ieee.org/abstract/document/9392337))
-*Jiunn-Kai Huang, Maani Ghaffari, Ross Hartley, Lu Gan, Ryan M. Eustice and Jessy W. Grizzle “LiDARTag: A Real-Time Fiducial Tag using Point Clouds” (under review)
+```
+@ARTICLE{HuangLiDARTag2020,
+ author={Huang, Jiunn-Kai and Wang, Shoutian and Ghaffari, Maani and Grizzle, Jessy W.},
+ journal={IEEE Robotics and Automation Letters},
+ title={LiDARTag: A Real-Time Fiducial Tag System for Point Clouds},
+ year={2021},
+ volume={6},
+ number={3},
+ pages={4875-4882},
+ doi={10.1109/LRA.2021.3070302}}
+```
diff --git a/cmake/FindTBB.cmake b/cmake/FindTBB.cmake
new file mode 100644
index 0000000..9128d34
--- /dev/null
+++ b/cmake/FindTBB.cmake
@@ -0,0 +1,304 @@
+# The MIT License (MIT)
+#
+# Copyright (c) 2015 Justus Calvin
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+#
+# FindTBB
+# -------
+#
+# Find TBB include directories and libraries.
+#
+# Usage:
+#
+# find_package(TBB [major[.minor]] [EXACT]
+# [QUIET] [REQUIRED]
+# [[COMPONENTS] [components...]]
+# [OPTIONAL_COMPONENTS components...])
+#
+# where the allowed components are tbbmalloc and tbb_preview. Users may modify
+# the behavior of this module with the following variables:
+#
+# * TBB_ROOT_DIR - The base directory the of TBB installation.
+# * TBB_INCLUDE_DIR - The directory that contains the TBB headers files.
+# * TBB_LIBRARY - The directory that contains the TBB library files.
+# * TBB__LIBRARY - The path of the TBB the corresponding TBB library.
+# These libraries, if specified, override the
+# corresponding library search results, where
+# may be tbb, tbb_debug, tbbmalloc, tbbmalloc_debug,
+# tbb_preview, or tbb_preview_debug.
+# * TBB_USE_DEBUG_BUILD - The debug version of tbb libraries, if present, will
+# be used instead of the release version.
+#
+# Users may modify the behavior of this module with the following environment
+# variables:
+#
+# * TBB_INSTALL_DIR
+# * TBBROOT
+# * LIBRARY_PATH
+#
+# This module will set the following variables:
+#
+# * TBB_FOUND - Set to false, or undefined, if we haven’t found, or
+# don’t want to use TBB.
+# * TBB__FOUND - If False, optional part of TBB sytem is
+# not available.
+# * TBB_VERSION - The full version string
+# * TBB_VERSION_MAJOR - The major version
+# * TBB_VERSION_MINOR - The minor version
+# * TBB_INTERFACE_VERSION - The interface version number defined in
+# tbb/tbb_stddef.h.
+# * TBB__LIBRARY_RELEASE - The path of the TBB release version of
+# , where may be tbb, tbb_debug,
+# tbbmalloc, tbbmalloc_debug, tbb_preview, or
+# tbb_preview_debug.
+# * TBB__LIBRARY_DEGUG - The path of the TBB release version of
+# , where may be tbb, tbb_debug,
+# tbbmalloc, tbbmalloc_debug, tbb_preview, or
+# tbb_preview_debug.
+#
+# The following varibles should be used to build and link with TBB:
+#
+# * TBB_INCLUDE_DIRS - The include directory for TBB.
+# * TBB_LIBRARIES - The libraries to link against to use TBB.
+# * TBB_LIBRARIES_RELEASE - The release libraries to link against to use TBB.
+# * TBB_LIBRARIES_DEBUG - The debug libraries to link against to use TBB.
+# * TBB_DEFINITIONS - Definitions to use when compiling code that uses
+# TBB.
+# * TBB_DEFINITIONS_RELEASE - Definitions to use when compiling release code that
+# uses TBB.
+# * TBB_DEFINITIONS_DEBUG - Definitions to use when compiling debug code that
+# uses TBB.
+#
+# This module will also create the "tbb" target that may be used when building
+# executables and libraries.
+
+include(FindPackageHandleStandardArgs)
+
+if(NOT TBB_FOUND)
+
+ ##################################
+ # Check the build type
+ ##################################
+
+ if(NOT DEFINED TBB_USE_DEBUG_BUILD)
+ if(CMAKE_BUILD_TYPE MATCHES "(Debug|DEBUG|debug|RelWithDebInfo|RELWITHDEBINFO|relwithdebinfo)")
+ set(TBB_BUILD_TYPE DEBUG)
+ else()
+ set(TBB_BUILD_TYPE RELEASE)
+ endif()
+ elseif(TBB_USE_DEBUG_BUILD)
+ set(TBB_BUILD_TYPE DEBUG)
+ else()
+ set(TBB_BUILD_TYPE RELEASE)
+ endif()
+
+ ##################################
+ # Set the TBB search directories
+ ##################################
+
+ # Define search paths based on user input and environment variables
+ set(TBB_SEARCH_DIR ${TBB_ROOT_DIR} $ENV{TBB_INSTALL_DIR} $ENV{TBBROOT})
+
+ # Define the search directories based on the current platform
+ if(CMAKE_SYSTEM_NAME STREQUAL "Windows")
+ set(TBB_DEFAULT_SEARCH_DIR "C:/Program Files/Intel/TBB"
+ "C:/Program Files (x86)/Intel/TBB")
+
+ # Set the target architecture
+ if(CMAKE_SIZEOF_VOID_P EQUAL 8)
+ set(TBB_ARCHITECTURE "intel64")
+ else()
+ set(TBB_ARCHITECTURE "ia32")
+ endif()
+
+ # Set the TBB search library path search suffix based on the version of VC
+ if(WINDOWS_STORE)
+ set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc11_ui")
+ elseif(MSVC14)
+ set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc14")
+ elseif(MSVC12)
+ set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc12")
+ elseif(MSVC11)
+ set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc11")
+ elseif(MSVC10)
+ set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc10")
+ endif()
+
+ # Add the library path search suffix for the VC independent version of TBB
+ list(APPEND TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc_mt")
+
+ elseif(CMAKE_SYSTEM_NAME STREQUAL "Darwin")
+ # OS X
+ set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb")
+
+ # TODO: Check to see which C++ library is being used by the compiler.
+ if(NOT ${CMAKE_SYSTEM_VERSION} VERSION_LESS 13.0)
+ # The default C++ library on OS X 10.9 and later is libc++
+ set(TBB_LIB_PATH_SUFFIX "lib/libc++" "lib")
+ else()
+ set(TBB_LIB_PATH_SUFFIX "lib")
+ endif()
+ elseif(CMAKE_SYSTEM_NAME STREQUAL "Linux")
+ # Linux
+ set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb")
+ # set(TBB_DEFAULT_SEARCH_DIR "/home/brucebot/softwares/intel_parallel_studio/tbb")
+
+ # TODO: Check compiler version to see the suffix should be /gcc4.1 or
+ # /gcc4.1. For now, assume that the compiler is more recent than
+ # gcc 4.4.x or later.
+ if(CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64")
+ set(TBB_LIB_PATH_SUFFIX "lib/intel64/gcc4.4")
+ elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "^i.86$")
+ set(TBB_LIB_PATH_SUFFIX "lib/ia32/gcc4.4")
+ endif()
+ endif()
+
+ ##################################
+ # Find the TBB include dir
+ ##################################
+
+ find_path(TBB_INCLUDE_DIRS tbb/tbb.h
+ HINTS ${TBB_INCLUDE_DIR} ${TBB_SEARCH_DIR}
+ PATHS ${TBB_DEFAULT_SEARCH_DIR}
+ PATH_SUFFIXES include)
+
+ ##################################
+ # Set version strings
+ ##################################
+
+ if(TBB_INCLUDE_DIRS)
+ file(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _tbb_version_file)
+ string(REGEX REPLACE ".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1"
+ TBB_VERSION_MAJOR "${_tbb_version_file}")
+ string(REGEX REPLACE ".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1"
+ TBB_VERSION_MINOR "${_tbb_version_file}")
+ string(REGEX REPLACE ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1"
+ TBB_INTERFACE_VERSION "${_tbb_version_file}")
+ set(TBB_VERSION "${TBB_VERSION_MAJOR}.${TBB_VERSION_MINOR}")
+ endif()
+
+ ##################################
+ # Find TBB components
+ ##################################
+
+ if(TBB_VERSION VERSION_LESS 4.3)
+ set(TBB_SEARCH_COMPOMPONENTS tbb_preview tbbmalloc tbb)
+ else()
+ set(TBB_SEARCH_COMPOMPONENTS tbb_preview tbbmalloc_proxy tbbmalloc tbb)
+ endif()
+
+ # Find each component
+ foreach(_comp ${TBB_SEARCH_COMPOMPONENTS})
+ if(";${TBB_FIND_COMPONENTS};tbb;" MATCHES ";${_comp};")
+
+ # Search for the libraries
+ find_library(TBB_${_comp}_LIBRARY_RELEASE ${_comp}
+ HINTS ${TBB_LIBRARY} ${TBB_SEARCH_DIR}
+ PATHS ${TBB_DEFAULT_SEARCH_DIR} ENV LIBRARY_PATH
+ PATH_SUFFIXES ${TBB_LIB_PATH_SUFFIX})
+
+ find_library(TBB_${_comp}_LIBRARY_DEBUG ${_comp}_debug
+ HINTS ${TBB_LIBRARY} ${TBB_SEARCH_DIR}
+ PATHS ${TBB_DEFAULT_SEARCH_DIR} ENV LIBRARY_PATH
+ PATH_SUFFIXES ${TBB_LIB_PATH_SUFFIX})
+
+ if(TBB_${_comp}_LIBRARY_DEBUG)
+ list(APPEND TBB_LIBRARIES_DEBUG "${TBB_${_comp}_LIBRARY_DEBUG}")
+ endif()
+ if(TBB_${_comp}_LIBRARY_RELEASE)
+ list(APPEND TBB_LIBRARIES_RELEASE "${TBB_${_comp}_LIBRARY_RELEASE}")
+ endif()
+ if(TBB_${_comp}_LIBRARY_${TBB_BUILD_TYPE} AND NOT TBB_${_comp}_LIBRARY)
+ set(TBB_${_comp}_LIBRARY "${TBB_${_comp}_LIBRARY_${TBB_BUILD_TYPE}}")
+ endif()
+
+ if(TBB_${_comp}_LIBRARY AND EXISTS "${TBB_${_comp}_LIBRARY}")
+ set(TBB_${_comp}_FOUND TRUE)
+ else()
+ set(TBB_${_comp}_FOUND FALSE)
+ endif()
+
+ # Mark internal variables as advanced
+ mark_as_advanced(TBB_${_comp}_LIBRARY_RELEASE)
+ mark_as_advanced(TBB_${_comp}_LIBRARY_DEBUG)
+ mark_as_advanced(TBB_${_comp}_LIBRARY)
+
+ endif()
+ endforeach()
+
+ ##################################
+ # Set compile flags and libraries
+ ##################################
+
+ set(TBB_DEFINITIONS_RELEASE "")
+ set(TBB_DEFINITIONS_DEBUG "-DTBB_USE_DEBUG=1")
+
+ if(TBB_LIBRARIES_${TBB_BUILD_TYPE})
+ set(TBB_DEFINITIONS "${TBB_DEFINITIONS_${TBB_BUILD_TYPE}}")
+ set(TBB_LIBRARIES "${TBB_LIBRARIES_${TBB_BUILD_TYPE}}")
+ elseif(TBB_LIBRARIES_RELEASE)
+ set(TBB_DEFINITIONS "${TBB_DEFINITIONS_RELEASE}")
+ set(TBB_LIBRARIES "${TBB_LIBRARIES_RELEASE}")
+ elseif(TBB_LIBRARIES_DEBUG)
+ set(TBB_DEFINITIONS "${TBB_DEFINITIONS_DEBUG}")
+ set(TBB_LIBRARIES "${TBB_LIBRARIES_DEBUG}")
+ endif()
+
+ find_package_handle_standard_args(TBB
+ REQUIRED_VARS TBB_INCLUDE_DIRS TBB_LIBRARIES
+ HANDLE_COMPONENTS
+ VERSION_VAR TBB_VERSION)
+
+ ##################################
+ # Create targets
+ ##################################
+
+ if(NOT CMAKE_VERSION VERSION_LESS 3.0 AND TBB_FOUND)
+ add_library(tbb SHARED IMPORTED)
+ set_target_properties(tbb PROPERTIES
+ INTERFACE_INCLUDE_DIRECTORIES ${TBB_INCLUDE_DIRS}
+ IMPORTED_LOCATION ${TBB_LIBRARIES})
+ if(TBB_LIBRARIES_RELEASE AND TBB_LIBRARIES_DEBUG)
+ set_target_properties(tbb PROPERTIES
+ INTERFACE_COMPILE_DEFINITIONS "$<$,$>:TBB_USE_DEBUG=1>"
+ IMPORTED_LOCATION_DEBUG ${TBB_LIBRARIES_DEBUG}
+ IMPORTED_LOCATION_RELWITHDEBINFO ${TBB_LIBRARIES_DEBUG}
+ IMPORTED_LOCATION_RELEASE ${TBB_LIBRARIES_RELEASE}
+ IMPORTED_LOCATION_MINSIZEREL ${TBB_LIBRARIES_RELEASE}
+ )
+ elseif(TBB_LIBRARIES_RELEASE)
+ set_target_properties(tbb PROPERTIES IMPORTED_LOCATION ${TBB_LIBRARIES_RELEASE})
+ else()
+ set_target_properties(tbb PROPERTIES
+ INTERFACE_COMPILE_DEFINITIONS "${TBB_DEFINITIONS_DEBUG}"
+ IMPORTED_LOCATION ${TBB_LIBRARIES_DEBUG}
+ )
+ endif()
+ endif()
+
+ mark_as_advanced(TBB_INCLUDE_DIRS TBB_LIBRARIES)
+
+ unset(TBB_ARCHITECTURE)
+ unset(TBB_BUILD_TYPE)
+ unset(TBB_LIB_PATH_SUFFIX)
+ unset(TBB_DEFAULT_SEARCH_DIR)
+
+endif()
diff --git a/figure/ClusterRemoval.png b/figure/ClusterRemoval.png
new file mode 100644
index 0000000..6986beb
Binary files /dev/null and b/figure/ClusterRemoval.png differ
diff --git a/figure/CompletelyDark.gif b/figure/CompletelyDark.gif
new file mode 100644
index 0000000..d7e3839
Binary files /dev/null and b/figure/CompletelyDark.gif differ
diff --git a/figure/Dingy.gif b/figure/Dingy.gif
new file mode 100644
index 0000000..e13e2ae
Binary files /dev/null and b/figure/Dingy.gif differ
diff --git a/figure/DoubleSum.png b/figure/DoubleSum.png
new file mode 100644
index 0000000..83f01d5
Binary files /dev/null and b/figure/DoubleSum.png differ
diff --git a/figure/FalsePositives.png b/figure/FalsePositives.png
new file mode 100644
index 0000000..aafeb81
Binary files /dev/null and b/figure/FalsePositives.png differ
diff --git a/figure/HalfOverExposured.gif b/figure/HalfOverExposured.gif
new file mode 100644
index 0000000..19134c8
Binary files /dev/null and b/figure/HalfOverExposured.gif differ
diff --git a/figure/LiDARTagAnalysis.png b/figure/LiDARTagAnalysis.png
new file mode 100644
index 0000000..abc7800
Binary files /dev/null and b/figure/LiDARTagAnalysis.png differ
diff --git a/figure/LiDARTagFrontView.png b/figure/LiDARTagFrontView.png
new file mode 100644
index 0000000..1160227
Binary files /dev/null and b/figure/LiDARTagFrontView.png differ
diff --git a/figure/LiDARTagIntro.png b/figure/LiDARTagIntro.png
new file mode 100644
index 0000000..153749e
Binary files /dev/null and b/figure/LiDARTagIntro.png differ
diff --git a/figure/LiDARTagSideView.png b/figure/LiDARTagSideView.png
new file mode 100644
index 0000000..baaa310
Binary files /dev/null and b/figure/LiDARTagSideView.png differ
diff --git a/figure/LiDARTagVideoImg.png b/figure/LiDARTagVideoImg.png
new file mode 100644
index 0000000..97a36b5
Binary files /dev/null and b/figure/LiDARTagVideoImg.png differ
diff --git a/figure/MarkerExample2.png b/figure/MarkerExample2.png
new file mode 100644
index 0000000..fdaa510
Binary files /dev/null and b/figure/MarkerExample2.png differ
diff --git a/figure/PipelineSpeedAnalysis.png b/figure/PipelineSpeedAnalysis.png
new file mode 100644
index 0000000..c036b20
Binary files /dev/null and b/figure/PipelineSpeedAnalysis.png differ
diff --git a/figure/Pipelinev3.png b/figure/Pipelinev3.png
new file mode 100644
index 0000000..23260f2
Binary files /dev/null and b/figure/Pipelinev3.png differ
diff --git a/figure/Pose.png b/figure/Pose.png
new file mode 100644
index 0000000..54b6902
Binary files /dev/null and b/figure/Pose.png differ
diff --git a/figure/RapidShort.gif b/figure/RapidShort.gif
new file mode 100644
index 0000000..82363bb
Binary files /dev/null and b/figure/RapidShort.gif differ
diff --git a/figure/first_fig1.png b/figure/first_fig1.png
new file mode 100644
index 0000000..ff49232
Binary files /dev/null and b/figure/first_fig1.png differ
diff --git a/figure/first_fig2.jpg b/figure/first_fig2.jpg
new file mode 100644
index 0000000..b3bd2bd
Binary files /dev/null and b/figure/first_fig2.jpg differ
diff --git a/figure/pipeline.png b/figure/pipeline.png
new file mode 100644
index 0000000..227fee4
Binary files /dev/null and b/figure/pipeline.png differ
diff --git a/if_no_ring.odt b/if_no_ring.odt
new file mode 100644
index 0000000..ea7d5d5
Binary files /dev/null and b/if_no_ring.odt differ
diff --git a/include/KDTreeVectorOfVectorsAdaptor.h b/include/KDTreeVectorOfVectorsAdaptor.h
new file mode 100644
index 0000000..0d4cef4
--- /dev/null
+++ b/include/KDTreeVectorOfVectorsAdaptor.h
@@ -0,0 +1,117 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2011-16 Jose Luis Blanco (joseluisblancoc@gmail.com).
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *************************************************************************/
+
+#pragma once
+
+#include "nanoflann.hpp"
+
+#include
+
+// ===== This example shows how to use nanoflann with these types of containers: =======
+//typedef std::vector > my_vector_of_vectors_t;
+//typedef std::vector my_vector_of_vectors_t; // This requires #include
+// =====================================================================================
+
+
+/** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage.
+ * The i'th vector represents a point in the state space.
+ *
+ * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations.
+ * \tparam num_t The type of the point coordinates (typically, double or float).
+ * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
+ * \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int)
+ */
+template
+struct KDTreeVectorOfVectorsAdaptor
+{
+ typedef KDTreeVectorOfVectorsAdaptor self_t;
+ typedef typename Distance::template traits::distance_t metric_t;
+ typedef nanoflann::KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t;
+
+ index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index.
+
+ /// Constructor: takes a const ref to the vector of vectors object with the data points
+ KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const VectorOfVectorsType &mat, const int leaf_max_size = 10) : m_data(mat)
+ {
+ assert(mat.size() != 0 && mat[0].size() != 0);
+ const size_t dims = mat[0].size();
+ if (DIM>0 && static_cast(dims) != DIM)
+ throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument");
+ index = new index_t( static_cast(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) );
+ index->buildIndex();
+ }
+
+ ~KDTreeVectorOfVectorsAdaptor() {
+ delete index;
+ }
+
+ const VectorOfVectorsType &m_data;
+
+ /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]).
+ * Note that this is a short-cut method for index->findNeighbors().
+ * The user can also call index->... methods as desired.
+ * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
+ */
+ inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const
+ {
+ nanoflann::KNNResultSet resultSet(num_closest);
+ resultSet.init(out_indices, out_distances_sq);
+ index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
+ }
+
+ /** @name Interface expected by KDTreeSingleIndexAdaptor
+ * @{ */
+
+ const self_t & derived() const {
+ return *this;
+ }
+ self_t & derived() {
+ return *this;
+ }
+
+ // Must return the number of data points
+ inline size_t kdtree_get_point_count() const {
+ return m_data.size();
+ }
+
+ // Returns the dim'th component of the idx'th point in the class:
+ inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const {
+ return m_data[idx][dim];
+ }
+
+ // Optional bounding-box computation: return false to default to a standard bbox computation loop.
+ // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
+ // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
+ template
+ bool kdtree_get_bbox(BBOX & /*bb*/) const {
+ return false;
+ }
+
+ /** @} */
+
+}; // end of KDTreeVectorOfVectorsAdaptor
diff --git a/include/apriltag_utils.h b/include/apriltag_utils.h
index 2651857..08e8d66 100644
--- a/include/apriltag_utils.h
+++ b/include/apriltag_utils.h
@@ -31,7 +31,7 @@
#ifndef APRILTAG_UTILS_H
#define APRILTAG_UTILS_H
-#include "lidar_tag.h"
+#include "lidartag.h"
namespace BipedAprilLab{
diff --git a/include/lidar_tag.h b/include/lidartag.h
similarity index 52%
rename from include/lidar_tag.h
rename to include/lidartag.h
index 5a44afd..96ece4b 100644
--- a/include/lidar_tag.h
+++ b/include/lidartag.h
@@ -36,6 +36,9 @@
#include // std::queue
#include
+#include
+#include
+#include
#include
#include
@@ -44,7 +47,10 @@
#include // Marker
#include // Marker
+
+// threadings
#include
+#include
// To trasnform to pcl format
#include
@@ -53,14 +59,19 @@
#include
#include
#include
+#include
#include
#include
+
#include "lidartag_msgs/LiDARTagDetection.h"
#include "lidartag_msgs/LiDARTagDetectionArray.h"
#include "types.h"
+#include "thread_pool.h"
+// #include "mat.h"
+// #include "matrix.h"
// #include "tensorflow_ros_test/lib.h"
namespace BipedLab{
@@ -80,9 +91,23 @@ namespace BipedLab{
int _valgrind_check; // Debugging with Valgrind
int _fake_tag;
int _decode_method; // Which decode methods to use?
+
+
+ int _optimization_solver;
+ int _decode_mode;
int _grid_viz; // visualize remapping grid
+ bool _mark_cluster_validity;
+ bool _plane_fitting; // whether perform plane fitting
+ bool _pose_optimization; // optimize pose or not
bool _id_decoding; // decode id or not
bool _write_CSV; // Write CSV files
+ bool _calibration;
+ bool _has_ring; // data has ring_num or not
+ bool _ring_estimation; // need to estimate ring_num or not
+ bool _ring_estimated;
+ bool _use_ring; // use ring information or not
+ int _num_accumulation; // Accumuate # of scans as a full scan of lidar
+ int _iter; //iterations of frame
std::string _assign_id; // Directly assign Id, mainly for calibration usage
// ROS
@@ -90,15 +115,31 @@ namespace BipedLab{
ros::Subscriber _LiDAR1_sub;
ros::Publisher _original_pc_pub;
ros::Publisher _edge_pub;
+ ros::Publisher _transformed_points_pub;
+ ros::Publisher _transformed_points_tag_pub;
+ ros::Publisher _edge1_pub;
+ ros::Publisher _edge2_pub;
+ ros::Publisher _edge3_pub;
+ ros::Publisher _edge4_pub;
+ ros::Publisher _boundary_pub;
ros::Publisher _cluster_pub;
ros::Publisher _payload_pub;
-
+ ros::Publisher _payload3d_pub;
+ ros::Publisher _tag_pub;
+ ros::Publisher _ini_tag_pub;
+ // ros::Publisher _index_pub;
ros::Publisher _cluster_marker_pub; // cluster markers
ros::Publisher _boundary_marker_pub; // cluster boundary
+ ros::Publisher _id_marker_pub;
ros::Publisher _payload_marker_pub; // payload boundary
ros::Publisher _payload_grid_pub; // grid visualization
ros::Publisher _payload_grid_line_pub; // grid visualization
+ ros::Publisher _ideal_frame_pub;
+ ros::Publisher _tag_frame_pub;
+ ros::Publisher _edge_vector_pub;
ros::Publisher _lidartag_pose_pub; // Publish LiDAR pose
+ ros::Publisher _clustered_points_pub; // Points after minor clusters removed
+ ros::Publisher _detectionArray_pub;
// Flag
int _point_cloud_received; // check if a scan of point cloud has received or not
@@ -112,24 +153,19 @@ namespace BipedLab{
//ros::Publisher TextPub_; // publish text
// visualization_msgs::MarkerArray _markers;
-
// Queue for pc data
std::queue _point_cloud1_queue;
- // lock
- boost::mutex _refine_lock;
- boost::mutex _buff_to_pcl_vector_lock;
- boost::mutex _point_cloud1_queue_lock;
// LiDAR parameters
ros::Time _current_scan_time; // store current time of the lidar scan
std::string _pointcloud_topic; // subscribe channel
std::string _pub_frame; // publish under what frame?
-
+ std::string lidartag_detection_topic;
// Overall LiDAR system parameters
LiDARSystem_t _LiDAR_system;
int _beam_num;
- int _vertical_fov;
+ double _vertical_fov;
// PointCould data (for a single scan)
int _point_cloud_size;
@@ -141,6 +177,7 @@ namespace BipedLab{
// If on-board processing is limited, limit range of points
double _distance_bound;
+ double _distance_threshold;
// fiducial marker parameters
double _payload_size; // physical payload size
@@ -148,21 +185,59 @@ namespace BipedLab{
int _tag_hamming_distance; // what tag family ie tag16h5, this should be 5
int _max_decode_hamming; // max hamming while decoding
int _black_border; // black boarder of the fiducial marker
+ int _num_codes;
+ int _num_tag_sizes;
+ std::vector> _function_dic;
+ std::vector> _function_dic_xyz; // for kd tree
+ std::vector> _function_dic_feat; // for kdtree
+ std::vector> _rkhs_function_name_list;
// Cluster parameters
- double _threshold;
+ double _linkage_threshold;
double _RANSAC_threshold;
int _fine_cluster_threshold; // TODO: REPLACE WITH TAG PARAMETERS
int _filling_gap_max_index; // TODO: CHECK
int _filling_max_points_threshold; //TODO: REMOVE
double _points_threshold_factor; // TODO: CHECK
-
+ double _distance_to_plane_threshold;
+ double _max_outlier_ratio;
+ int _num_points_for_plane_feature;
+ double _nearby_factor;
+ int _minimum_ring_boundary_points;
+ int _np_ring;
+ double _linkage_tunable;
+ std::vector _tag_size_list;
+ double _optimization_percent;
+
+ bool _print_ros_info;
+ bool _debug_info;
+ bool _debug_time;
+ bool _debug_decoding_time;
+ bool _log_data;
+ bool _derivative_method;
// Payload
double _line_intensity_bound; // TODO: not sure but may okay to remove it for releasing
double _payload_intensity_threshold;
+ double _opt_lb;
+ double _opt_ub;
+ double _coa_tunable;
+ double _tagsize_tunable;
int _min_returns_per_grid;
GrizTagFamily_t *tf;
lidartag_msgs::LiDARTagDetectionArray _lidartag_pose_array; // an array of apriltags
+ lidartag_msgs::LiDARTagDetectionArray detectionsToPub;
+
+
+
+ // threadings
+ int _num_threads;
+ std::shared_ptr _thread_vec;
+
+
+ // lock
+ boost::mutex _refine_lock;
+ boost::mutex _buff_to_pcl_vector_lock;
+ boost::mutex _point_cloud1_queue_lock;
// NN
@@ -175,10 +250,15 @@ namespace BipedLab{
// Debug
Debug_t _debug_cluster;
Timing_t _timing;
+ TimeDecoding_t _time_decoding;
// Statistics
Statistics_t _result_statistics;
-
+ std::string _stats_file_path;
+ std::string _cluster_file_path;
+ std::string _pose_file_path;
+ std::string _outputs_path;
+ std::string _library_path;
/*****************************************************
* Functions
@@ -220,11 +300,12 @@ namespace BipedLab{
/* [Transform/Publish]
* A function to transform pcl msgs to ros msgs and then publish
- * WhichPublisher should be a string of "organized" or "original" regardless
- * lowercase and uppercase
+ * WhichPublisher should be a string of "organized" or "original"
+ * regardless lowercase and uppercase
*/
- void _publishPC(const pcl::PointCloud::Ptr &t_source_PC,
- const std::string &t_frame_name, std::string t_which_publisher);
+ void _publishPC(const pcl::PointCloud::Ptr &t_source_PC,
+ const std::string &t_frame_name,
+ std::string t_which_publisher);
/* [Type transformation]
* A function to transform from a customized type (LiDARpoints_t) of vector of
@@ -239,8 +320,15 @@ namespace BipedLab{
*/
inline void _fillInOrderedPC(const pcl::PointCloud::Ptr &t_pcl_pointcloud,
std::vector< std::vector > &t_ordered_buff);
-
-
+ /*
+ * A function to compute angle between the line from origin to this point
+ * and z=0 plane in lidar
+ * */
+ float _getAnglefromPt(PointXYZRI &t_point);
+
+ void _getAngleVector(
+ const pcl::PointCloud::Ptr &pcl_pointcloud,
+ std::vector &angles);
/* [Type transformation]
* A function to get pcl OrderedBuff from a ros sensor-msgs form of
* pointcould queue
@@ -299,6 +387,17 @@ namespace BipedLab{
std::vector> &t_edge_buff,
std::vector &t_cluster_buff);
+ /* [Edge detection from n consecutive points]
+ *
+ * A function to
+ * (1) kick out points are not near to each other
+ * (2) find edge points from two side points of these points
+ * Return value : 0 mean no edge point, 1 mean the left side point is the edge point
+ * Return value : 2 mean the right side point is the edge point, 3 mean two side points are edge points
+ */
+ int _getEdgePoints(const std::vector>& OrderedBuff,
+ int i, int j, int n);
+
/* [Clustering-Linkage]
* A function to cluster a single edge point into a new cluster or an existing cluster
@@ -315,7 +414,17 @@ namespace BipedLab{
ClusterFamily_t &t_old_cluster,
TestCluster_t *t_new_cluster);
+ /* [Adaptive-Clustering]
+ * A function that determines if a point is within a given cluster adaptively
+ * based on the ring number and range of the point.
+ */
+ bool _isWithinCluster(const LiDARPoints_t &point, ClusterFamily_t &cluster);
+ /* [Adaptive-Clustering]
+ * A function that determines if a point is within a given cluster horizontally and adaptively
+ * based on the range of the point.
+ */
+ bool _isWithinClusterHorizon(const LiDARPoints_t &point, ClusterFamily_t &cluster, double threshold);
/* [Clustering-Validation]
* A function to
@@ -371,21 +480,77 @@ namespace BipedLab{
*/
void _extractPayloadWOThreshold(ClusterFamily_t &t_cluster);
+ /*
+ * A function to calculate the average point of valid edge points
+ */
+ void _organizeDataPoints(ClusterFamily_t &t_cluster);
+ /* [Edge points and principal axes]
+ * A function to transform the edge points to the tag frame
+ */
+ bool _transformSplitEdges(ClusterFamily_t &t_cluster);
+ /*
+ * A function to store transformed points
+ */
+ void _storeTemplatePts(ClusterFamily_t &t_Cluster);
+
+
+ /* [Unordered corner points]
+ * A function to reorder the undered corner points from PCA
+ */
+ Eigen::MatrixXf _getOrderedCorners(Eigen::MatrixXf &t_payload_vertices, ClusterFamily_t &t_Cluster);
+
+ /* [two lines]
+ * A function to compute the intersection of two lines
+ */
+ Eigen::Vector3f _getintersec(Eigen::Vector4f t_line1, Eigen::Vector4f t_line2);
+
+ /* [four corner points]
+ * A function to compute tag size according to the corner points of the tag
+ */
+ bool _estimateTargetSize(
+ ClusterFamily_t &t_cluster,
+ const Eigen::Vector3f &point1,
+ const Eigen::Vector3f &point2,
+ const Eigen::Vector3f &point3,
+ const Eigen::Vector3f &point4);
+
+ /* [A set of 2D points]
+ * A function to transform the edge points to the tag frame
+ */
+ bool _getLines(pcl::PointCloud::Ptr t_cloud, Eigen::Vector4f &t_line);
/* [Type transformation]
* A function to transform an eigen type of vector to pcl point type
*/
void _eigenVectorToPointXYZRI(const Eigen::Vector4f &t_vector, PointXYZRI &t_point);
+ /* [Type transformation]
+ * A function to transform a pcl point type to an eigen vector
+ */
+ void _PointXYZRIToEigenVector(const PointXYZRI &point, Eigen::Vector4f &Vector);
+
/* [Normal vector]
* A function to estimate the normal vector of a potential payload
*/
- Eigen::MatrixXf _estimateNormalVector(ClusterFamily_t &cluster);
+ //Eigen::MatrixXf
+ void _estimatePrincipleAxis(ClusterFamily_t &cluster);
+ /* [pose]
+ * A function to estimate the pose of a potential payload
+ */
+ Homogeneous_t _estimatePose(ClusterFamily_t &cluster);
+ /*[oritented vector]
+ */
+ Eigen::Vector3f _estimateEdgeVector(ClusterFamily_t &Cluster);
+ /* [pose]
+ * A function to optimize the pose of a potential payload with L1 optimization
+ */
+ int _optimizePose(ClusterFamily_t &cluster);
+ bool _optimizePoseGrad(ClusterFamily_t &cluster);
/* [Pose: tag to robot]
* A function to publish pose of tag to the robot
*/
@@ -402,7 +567,7 @@ namespace BipedLab{
* 3: Gaussian Process
* 4: ?!
*/
- bool _decodPayload(ClusterFamily_t &t_cluster);
+ bool _decodePayload(ClusterFamily_t &t_cluster);
/* [Decoder]
@@ -424,6 +589,186 @@ namespace BipedLab{
const pcl::PointCloud &t_payload,
const std::vector &t_payload_boundary_ptr);
+ /* [Decoder]
+ * 1) Transfrom the payload points to 3D-shape pc
+ * 2) Compute inner product
+ * 3)
+ */
+ int _getCodeRKHS(RKHSDecoding_t &rkhs_decoding, const double &tag_size);
+
+
+ Eigen::MatrixXf _construct3DShapeMarker(RKHSDecoding_t &rkhs_decoding,
+ const double &ell);
+
+ float computeFunctionInnerProduct(
+ const Eigen::MatrixXf &pc1,
+ const Eigen::MatrixXf &pc2,
+ const float &ell);
+
+
+ void computeFunctionOriginalInnerProduct(
+ const Eigen::MatrixXf &pc1,
+ const float &num_pc1,
+ const Eigen::MatrixXf &pc2,
+ const float &num_pc2,
+ const float &geo_sig,
+ const float &feature_ell,
+ const float &geo_ell,
+ float &score);
+
+
+ void computeFunctionMatrixInnerProduct(
+ const Eigen::MatrixXf &pc1,
+ const float &num_pc1,
+ const Eigen::MatrixXf &pc2,
+ const float &num_pc2,
+ const float &geo_sig,
+ const float &feature_ell,
+ const float &geo_ell,
+ float &score);
+
+
+ void computeFunctionVectorInnerProduct(
+ const Eigen::MatrixXf &pc1,
+ const float &num_pc1,
+ const Eigen::MatrixXf &pc2,
+ const float &num_pc2,
+ const float &geo_sig,
+ const float &feature_ell,
+ const float &geo_ell,
+ float &score);
+
+ void _assignClusterPose(
+ const Homogeneous_t &H_TL,
+ Homogeneous_t &H_LT,
+ const int &rotation_angle);
+
+
+ void singleTask(const Eigen::ArrayXf &x_ary,
+ const Eigen::ArrayXf &y_ary,
+ const Eigen::ArrayXf &z_ary,
+ const Eigen::ArrayXf &i_ary,
+ const Eigen::MatrixXf &pc1_j,
+ const float &geo_sig,
+ const float &feature_ell,
+ const float &geo_ell,
+ float &score);
+
+ void singleTaskFixedSize(
+ const Eigen::ArrayXf &x_ary,
+ const Eigen::ArrayXf &y_ary,
+ const Eigen::ArrayXf &z_ary,
+ const Eigen::ArrayXf &i_ary,
+ const Eigen::MatrixXf &pc1_j,
+ const float &geo_sig,
+ const float &feature_ell,
+ const float &geo_ell,
+ float &score);
+
+
+ void multipleTasks(
+ const Eigen::ArrayXf &x_ary,
+ const Eigen::ArrayXf &y_ary,
+ const Eigen::ArrayXf &z_ary,
+ const Eigen::ArrayXf &i_ary,
+ const Eigen::MatrixXf &pc1_j,
+ const float &geo_sig,
+ const float &feature_ell,
+ const float &geo_ell,
+ float &score);
+
+
+ void computeFunctionVectorInnerProductThreading(
+ const Eigen::MatrixXf &pc1,
+ const int &num_pc1,
+ const Eigen::MatrixXf &pc2,
+ const int &num_pc2,
+ const float &geo_sig,
+ const float &feature_ell,
+ const float &geo_ell,
+ float &score);
+
+ void test(
+ const Eigen::ArrayXf &x_ary,
+ const Eigen::ArrayXf &y_ary,
+ const Eigen::ArrayXf &z_ary,
+ const Eigen::ArrayXf &i_ary,
+ const Eigen::MatrixXf &pc1_j,
+ const float &geo_sig,
+ const float &feature_ell,
+ const float &geo_ell,
+ float &score);
+
+ void computeFunctionVectorInnerProductTBBThreadingNoScheduling(
+ const Eigen::MatrixXf &pc1,
+ const int &num_pc1,
+ const Eigen::MatrixXf &pc2,
+ const int &num_pc2,
+ const float &geo_sig,
+ const float &feature_ell,
+ const float &geo_ell,
+ float &score);
+ void computeFunctionVectorInnerProductTBBThreadingManualScheduling(
+ const Eigen::MatrixXf &pc1,
+ const int &num_pc1,
+ const Eigen::MatrixXf &pc2,
+ const int &num_pc2,
+ const float &geo_sig,
+ const float &feature_ell,
+ const float &geo_ell,
+ float &score);
+
+ void computeFunctionVectorInnerProductTBBThreadingTBBScheduling(
+ const Eigen::MatrixXf &pc1,
+ const int &num_pc1,
+ const Eigen::MatrixXf &pc2,
+ const int &num_pc2,
+ const float &geo_sig,
+ const float &feature_ell,
+ const float &geo_ell,
+ float &score);
+
+ void computeFunctionOriginalInnerProductTBB(
+ const Eigen::MatrixXf &pc1,
+ const float &num_pc1,
+ const Eigen::MatrixXf &pc2,
+ const float &num_pc2,
+ const float &geo_sig,
+ const float &feature_ell,
+ const float &geo_ell,
+ float &score);
+
+ void computeFunctionOriginalInnerProductKDTree(
+ const Eigen::MatrixXf &pc1,
+ const int &num_pc1,
+ const Eigen::MatrixXf &pc2,
+ const int &num_pc2,
+ const float &geo_sig,
+ const float &feature_ell,
+ const float &geo_ell,
+ float &score);
+
+ void computeFunctionInnerProductModes(
+ const int mode,
+ const Eigen::MatrixXf &pc1,
+ const float &num_pc1,
+ const Eigen::MatrixXf &pc2,
+ const float &num_pc2,
+ const float &geo_sig,
+ const float &feature_ell,
+ const float &geo_ell,
+ float &score);
+
+
+
+
+
+
+ // Eigen::VectorXf d_px_euler(double x11, double y11, double z11, double rpy11, double rpy12, double rpy13);
+ // Eigen::VectorXf d_py_euler(double x11, double y11, double z11, double rpy11, double rpy12, double rpy13);
+ // Eigen::VectorXf d_pz_euler(double x11, double y11, double z11, double rpy11, double rpy12, double rpy13);
+
+
/* [Decoder]
* Create hash table of chosen tag family
*/
@@ -441,10 +786,34 @@ namespace BipedLab{
*/
void _clusterToPclVectorAndMarkerPublisher(const std::vector &t_cluster,
pcl::PointCloud::Ptr t_out_cluster,
+ pcl::PointCloud::Ptr t_out_edge_cluster,
pcl::PointCloud::Ptr t_out_payload,
+ pcl::PointCloud::Ptr t_out_payload3d,
+ pcl::PointCloud::Ptr t_out_target,
+ pcl::PointCloud::Ptr t_ini_out_target,
+ pcl::PointCloud::Ptr t_edge1,
+ pcl::PointCloud::Ptr t_edge2,
+ pcl::PointCloud::Ptr t_edge3,
+ pcl::PointCloud::Ptr t_edge4,
+ pcl::PointCloud::Ptr t_boundary_pts,
visualization_msgs::MarkerArray &t_marker_array);
+ void _plotIdealFrame();
+ void _plotTagFrame(const ClusterFamily_t &t_cluster);
+ visualization_msgs::Marker _visualizeVector(Eigen::Vector3f edge_vector, PointXYZRI centriod, int t_ID);
+ /* [accumulating temporal cluster]
+ * A function to save temporal clusters data
+ */
+ // void _saveTemporalCluster(const std::vector &t_cluster, std::vector>> &matData);
+
+ /* [save points to mat files]
+ * A function to save points as .mat data
+ */
+ // void _saveMatFiles(std::vector>>& matData);
+
+ // [A function to put clusterFamily to LiDARTagDetectionArray]
+ void _detectionArrayPublisher(const ClusterFamily_t &Cluster);
-
+
/* [Drawing]
* A function to draw lines in rviz
*/
@@ -465,6 +834,30 @@ namespace BipedLab{
const PointXYZRI &t_point, const int t_count,
const double t_size, const std::string Text="");
+ void _assignVectorMarker(visualization_msgs::Marker &t_marker, const uint32_t t_shape,
+ const std::string t_namespace,
+ const double r, const double g, const double b,
+ const int t_count, const double t_size, Eigen::Vector3f t_edge_vector, const PointXYZRI &t_centriod,
+ const std::string Text="");
+
+ void _printStatistics(
+ const std::vector& ClusterBuff);
+
+ std::vector _getValidClusters(const std::vector& ClusterBuff);
+
+ void _maxPointsCheck(ClusterFamily_t& Cluster);
+
+ bool _rejectWithPlanarCheck( ClusterFamily_t &Cluster,
+ pcl::PointIndices::Ptr inliers, pcl::ModelCoefficients::Ptr coefficients,
+ std::ostream &fplanefit);
+
+ void _initFunctionDecoder();
+
+
+ ///
+ // void _calculateCost(Eigen::Vector3f q, double & costx, double & costy, double & costz);
+ // double _checkCost(double point, double cons1, double cons2);
+ // double _costfunc(const std::vector &x, std::vector &grad, void *func_data);
/*****************************************************
* not used
diff --git a/include/nanoflann.hpp b/include/nanoflann.hpp
new file mode 100644
index 0000000..ba0309c
--- /dev/null
+++ b/include/nanoflann.hpp
@@ -0,0 +1,2049 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ * Copyright 2011-2016 Jose Luis Blanco (joseluisblancoc@gmail.com).
+ * All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *************************************************************************/
+
+/** \mainpage nanoflann C++ API documentation
+ * nanoflann is a C++ header-only library for building KD-Trees, mostly
+ * optimized for 2D or 3D point clouds.
+ *
+ * nanoflann does not require compiling or installing, just an
+ * #include in your code.
+ *
+ * See:
+ * - C++ API organized by modules
+ * - Online README
+ * - Doxygen
+ * documentation
+ */
+
+#ifndef NANOFLANN_HPP_
+#define NANOFLANN_HPP_
+
+#include
+#include
+#include
+#include // for abs()
+#include // for fwrite()
+#include // for abs()
+#include
+#include // std::reference_wrapper
+#include
+#include
+
+/** Library version: 0xMmP (M=Major,m=minor,P=patch) */
+#define NANOFLANN_VERSION 0x132
+
+// Avoid conflicting declaration of min/max macros in windows headers
+#if !defined(NOMINMAX) && \
+ (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64))
+#define NOMINMAX
+#ifdef max
+#undef max
+#undef min
+#endif
+#endif
+
+namespace nanoflann {
+/** @addtogroup nanoflann_grp nanoflann C++ library for ANN
+ * @{ */
+
+/** the PI constant (required to avoid MSVC missing symbols) */
+template T pi_const() {
+ return static_cast(3.14159265358979323846);
+}
+
+/**
+ * Traits if object is resizable and assignable (typically has a resize | assign
+ * method)
+ */
+template struct has_resize : std::false_type {};
+
+template
+struct has_resize().resize(1), 0)>
+ : std::true_type {};
+
+template struct has_assign : std::false_type {};
+
+template
+struct has_assign().assign(1, 0), 0)>
+ : std::true_type {};
+
+/**
+ * Free function to resize a resizable object
+ */
+template
+inline typename std::enable_if::value, void>::type
+resize(Container &c, const size_t nElements) {
+ c.resize(nElements);
+}
+
+/**
+ * Free function that has no effects on non resizable containers (e.g.
+ * std::array) It raises an exception if the expected size does not match
+ */
+template
+inline typename std::enable_if::value, void>::type
+resize(Container &c, const size_t nElements) {
+ if (nElements != c.size())
+ throw std::logic_error("Try to change the size of a std::array.");
+}
+
+/**
+ * Free function to assign to a container
+ */
+template
+inline typename std::enable_if::value, void>::type
+assign(Container &c, const size_t nElements, const T &value) {
+ c.assign(nElements, value);
+}
+
+/**
+ * Free function to assign to a std::array
+ */
+template
+inline typename std::enable_if::value, void>::type
+assign(Container &c, const size_t nElements, const T &value) {
+ for (size_t i = 0; i < nElements; i++)
+ c[i] = value;
+}
+
+/** @addtogroup result_sets_grp Result set classes
+ * @{ */
+template
+class KNNResultSet {
+public:
+ typedef _DistanceType DistanceType;
+ typedef _IndexType IndexType;
+ typedef _CountType CountType;
+
+private:
+ IndexType *indices;
+ DistanceType *dists;
+ CountType capacity;
+ CountType count;
+
+public:
+ inline KNNResultSet(CountType capacity_)
+ : indices(0), dists(0), capacity(capacity_), count(0) {}
+
+ inline void init(IndexType *indices_, DistanceType *dists_) {
+ indices = indices_;
+ dists = dists_;
+ count = 0;
+ if (capacity)
+ dists[capacity - 1] = (std::numeric_limits::max)();
+ }
+
+ inline CountType size() const { return count; }
+
+ inline bool full() const { return count == capacity; }
+
+ /**
+ * Called during search to add an element matching the criteria.
+ * @return true if the search should be continued, false if the results are
+ * sufficient
+ */
+ inline bool addPoint(DistanceType dist, IndexType index) {
+ CountType i;
+ for (i = count; i > 0; --i) {
+#ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same
+ // distance, the one with the lowest-index will be
+ // returned first.
+ if ((dists[i - 1] > dist) ||
+ ((dist == dists[i - 1]) && (indices[i - 1] > index))) {
+#else
+ if (dists[i - 1] > dist) {
+#endif
+ if (i < capacity) {
+ dists[i] = dists[i - 1];
+ indices[i] = indices[i - 1];
+ }
+ } else
+ break;
+ }
+ if (i < capacity) {
+ dists[i] = dist;
+ indices[i] = index;
+ }
+ if (count < capacity)
+ count++;
+
+ // tell caller that the search shall continue
+ return true;
+ }
+
+ inline DistanceType worstDist() const { return dists[capacity - 1]; }
+};
+
+/** operator "<" for std::sort() */
+struct IndexDist_Sorter {
+ /** PairType will be typically: std::pair */
+ template
+ inline bool operator()(const PairType &p1, const PairType &p2) const {
+ return p1.second < p2.second;
+ }
+};
+
+/**
+ * A result-set class used when performing a radius based search.
+ */
+template
+class RadiusResultSet {
+public:
+ typedef _DistanceType DistanceType;
+ typedef _IndexType IndexType;
+
+public:
+ const DistanceType radius;
+
+ std::vector> &m_indices_dists;
+
+ inline RadiusResultSet(
+ DistanceType radius_,
+ std::vector> &indices_dists)
+ : radius(radius_), m_indices_dists(indices_dists) {
+ init();
+ }
+
+ inline void init() { clear(); }
+ inline void clear() { m_indices_dists.clear(); }
+
+ inline size_t size() const { return m_indices_dists.size(); }
+
+ inline bool full() const { return true; }
+
+ /**
+ * Called during search to add an element matching the criteria.
+ * @return true if the search should be continued, false if the results are
+ * sufficient
+ */
+ inline bool addPoint(DistanceType dist, IndexType index) {
+ if (dist < radius)
+ m_indices_dists.push_back(std::make_pair(index, dist));
+ return true;
+ }
+
+ inline DistanceType worstDist() const { return radius; }
+
+ /**
+ * Find the worst result (furtherest neighbor) without copying or sorting
+ * Pre-conditions: size() > 0
+ */
+ std::pair worst_item() const {
+ if (m_indices_dists.empty())
+ throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on "
+ "an empty list of results.");
+ typedef
+ typename std::vector>::const_iterator
+ DistIt;
+ DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end(),
+ IndexDist_Sorter());
+ return *it;
+ }
+};
+
+/** @} */
+
+/** @addtogroup loadsave_grp Load/save auxiliary functions
+ * @{ */
+template
+void save_value(FILE *stream, const T &value, size_t count = 1) {
+ fwrite(&value, sizeof(value), count, stream);
+}
+
+template
+void save_value(FILE *stream, const std::vector &value) {
+ size_t size = value.size();
+ fwrite(&size, sizeof(size_t), 1, stream);
+ fwrite(&value[0], sizeof(T), size, stream);
+}
+
+template
+void load_value(FILE *stream, T &value, size_t count = 1) {
+ size_t read_cnt = fread(&value, sizeof(value), count, stream);
+ if (read_cnt != count) {
+ throw std::runtime_error("Cannot read from file");
+ }
+}
+
+template void load_value(FILE *stream, std::vector &value) {
+ size_t size;
+ size_t read_cnt = fread(&size, sizeof(size_t), 1, stream);
+ if (read_cnt != 1) {
+ throw std::runtime_error("Cannot read from file");
+ }
+ value.resize(size);
+ read_cnt = fread(&value[0], sizeof(T), size, stream);
+ if (read_cnt != size) {
+ throw std::runtime_error("Cannot read from file");
+ }
+}
+/** @} */
+
+/** @addtogroup metric_grp Metric (distance) classes
+ * @{ */
+
+struct Metric {};
+
+/** Manhattan distance functor (generic version, optimized for
+ * high-dimensionality data sets). Corresponding distance traits:
+ * nanoflann::metric_L1 \tparam T Type of the elements (e.g. double, float,
+ * uint8_t) \tparam _DistanceType Type of distance variables (must be signed)
+ * (e.g. float, double, int64_t)
+ */
+template
+struct L1_Adaptor {
+ typedef T ElementType;
+ typedef _DistanceType DistanceType;
+
+ const DataSource &data_source;
+
+ L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) {}
+
+ inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size,
+ DistanceType worst_dist = -1) const {
+ DistanceType result = DistanceType();
+ const T *last = a + size;
+ const T *lastgroup = last - 3;
+ size_t d = 0;
+
+ /* Process 4 items with each loop for efficiency. */
+ while (a < lastgroup) {
+ const DistanceType diff0 =
+ std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++));
+ const DistanceType diff1 =
+ std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++));
+ const DistanceType diff2 =
+ std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++));
+ const DistanceType diff3 =
+ std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++));
+ result += diff0 + diff1 + diff2 + diff3;
+ a += 4;
+ if ((worst_dist > 0) && (result > worst_dist)) {
+ return result;
+ }
+ }
+ /* Process last 0-3 components. Not needed for standard vector lengths. */
+ while (a < last) {
+ result += std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++));
+ }
+ return result;
+ }
+
+ template
+ inline DistanceType accum_dist(const U a, const V b, const size_t) const {
+ return std::abs(a - b);
+ }
+};
+
+/** Squared Euclidean distance functor (generic version, optimized for
+ * high-dimensionality data sets). Corresponding distance traits:
+ * nanoflann::metric_L2 \tparam T Type of the elements (e.g. double, float,
+ * uint8_t) \tparam _DistanceType Type of distance variables (must be signed)
+ * (e.g. float, double, int64_t)
+ */
+template
+struct L2_Adaptor {
+ typedef T ElementType;
+ typedef _DistanceType DistanceType;
+
+ const DataSource &data_source;
+
+ L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {}
+
+ inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size,
+ DistanceType worst_dist = -1) const {
+ DistanceType result = DistanceType();
+ const T *last = a + size;
+ const T *lastgroup = last - 3;
+ size_t d = 0;
+
+ /* Process 4 items with each loop for efficiency. */
+ while (a < lastgroup) {
+ const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx, d++);
+ const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx, d++);
+ const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx, d++);
+ const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx, d++);
+ result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
+ a += 4;
+ if ((worst_dist > 0) && (result > worst_dist)) {
+ return result;
+ }
+ }
+ /* Process last 0-3 components. Not needed for standard vector lengths. */
+ while (a < last) {
+ const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx, d++);
+ result += diff0 * diff0;
+ }
+ return result;
+ }
+
+ template
+ inline DistanceType accum_dist(const U a, const V b, const size_t) const {
+ return (a - b) * (a - b);
+ }
+};
+
+/** Squared Euclidean (L2) distance functor (suitable for low-dimensionality
+ * datasets, like 2D or 3D point clouds) Corresponding distance traits:
+ * nanoflann::metric_L2_Simple \tparam T Type of the elements (e.g. double,
+ * float, uint8_t) \tparam _DistanceType Type of distance variables (must be
+ * signed) (e.g. float, double, int64_t)
+ */
+template
+struct L2_Simple_Adaptor {
+ typedef T ElementType;
+ typedef _DistanceType DistanceType;
+
+ const DataSource &data_source;
+
+ L2_Simple_Adaptor(const DataSource &_data_source)
+ : data_source(_data_source) {}
+
+ inline DistanceType evalMetric(const T *a, const size_t b_idx,
+ size_t size) const {
+ DistanceType result = DistanceType();
+ for (size_t i = 0; i < size; ++i) {
+ const DistanceType diff = a[i] - data_source.kdtree_get_pt(b_idx, i);
+ result += diff * diff;
+ }
+ return result;
+ }
+
+ template
+ inline DistanceType accum_dist(const U a, const V b, const size_t) const {
+ return (a - b) * (a - b);
+ }
+};
+
+/** SO2 distance functor
+ * Corresponding distance traits: nanoflann::metric_SO2
+ * \tparam T Type of the elements (e.g. double, float)
+ * \tparam _DistanceType Type of distance variables (must be signed) (e.g.
+ * float, double) orientation is constrained to be in [-pi, pi]
+ */
+template
+struct SO2_Adaptor {
+ typedef T ElementType;
+ typedef _DistanceType DistanceType;
+
+ const DataSource &data_source;
+
+ SO2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {}
+
+ inline DistanceType evalMetric(const T *a, const size_t b_idx,
+ size_t size) const {
+ return accum_dist(a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1),
+ size - 1);
+ }
+
+ /** Note: this assumes that input angles are already in the range [-pi,pi] */
+ template
+ inline DistanceType accum_dist(const U a, const V b, const size_t) const {
+ DistanceType result = DistanceType();
+ DistanceType PI = pi_const();
+ result = b - a;
+ if (result > PI)
+ result -= 2 * PI;
+ else if (result < -PI)
+ result += 2 * PI;
+ return result;
+ }
+};
+
+/** SO3 distance functor (Uses L2_Simple)
+ * Corresponding distance traits: nanoflann::metric_SO3
+ * \tparam T Type of the elements (e.g. double, float)
+ * \tparam _DistanceType Type of distance variables (must be signed) (e.g.
+ * float, double)
+ */
+template
+struct SO3_Adaptor {
+ typedef T ElementType;
+ typedef _DistanceType DistanceType;
+
+ L2_Simple_Adaptor distance_L2_Simple;
+
+ SO3_Adaptor(const DataSource &_data_source)
+ : distance_L2_Simple(_data_source) {}
+
+ inline DistanceType evalMetric(const T *a, const size_t b_idx,
+ size_t size) const {
+ return distance_L2_Simple.evalMetric(a, b_idx, size);
+ }
+
+ template
+ inline DistanceType accum_dist(const U a, const V b, const size_t idx) const {
+ return distance_L2_Simple.accum_dist(a, b, idx);
+ }
+};
+
+/** Metaprogramming helper traits class for the L1 (Manhattan) metric */
+struct metric_L1 : public Metric {
+ template struct traits {
+ typedef L1_Adaptor distance_t;
+ };
+};
+/** Metaprogramming helper traits class for the L2 (Euclidean) metric */
+struct metric_L2 : public Metric {
+ template struct traits {
+ typedef L2_Adaptor distance_t;
+ };
+};
+/** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */
+struct metric_L2_Simple : public Metric {
+ template struct traits {
+ typedef L2_Simple_Adaptor distance_t;
+ };
+};
+/** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */
+struct metric_SO2 : public Metric {
+ template struct traits {
+ typedef SO2_Adaptor distance_t;
+ };
+};
+/** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */
+struct metric_SO3 : public Metric {
+ template struct traits {
+ typedef SO3_Adaptor distance_t;
+ };
+};
+
+/** @} */
+
+/** @addtogroup param_grp Parameter structs
+ * @{ */
+
+/** Parameters (see README.md) */
+struct KDTreeSingleIndexAdaptorParams {
+ KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10)
+ : leaf_max_size(_leaf_max_size) {}
+
+ size_t leaf_max_size;
+};
+
+/** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */
+struct SearchParams {
+ /** Note: The first argument (checks_IGNORED_) is ignored, but kept for
+ * compatibility with the FLANN interface */
+ SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true)
+ : checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {}
+
+ int checks; //!< Ignored parameter (Kept for compatibility with the FLANN
+ //!< interface).
+ float eps; //!< search for eps-approximate neighbours (default: 0)
+ bool sorted; //!< only for radius search, require neighbours sorted by
+ //!< distance (default: true)
+};
+/** @} */
+
+/** @addtogroup memalloc_grp Memory allocation
+ * @{ */
+
+/**
+ * Allocates (using C's malloc) a generic type T.
+ *
+ * Params:
+ * count = number of instances to allocate.
+ * Returns: pointer (of type T*) to memory buffer
+ */
+template inline T *allocate(size_t count = 1) {
+ T *mem = static_cast(::malloc(sizeof(T) * count));
+ return mem;
+}
+
+/**
+ * Pooled storage allocator
+ *
+ * The following routines allow for the efficient allocation of storage in
+ * small chunks from a specified pool. Rather than allowing each structure
+ * to be freed individually, an entire pool of storage is freed at once.
+ * This method has two advantages over just using malloc() and free(). First,
+ * it is far more efficient for allocating small objects, as there is
+ * no overhead for remembering all the information needed to free each
+ * object or consolidating fragmented memory. Second, the decision about
+ * how long to keep an object is made at the time of allocation, and there
+ * is no need to track down all the objects to free them.
+ *
+ */
+
+const size_t WORDSIZE = 16;
+const size_t BLOCKSIZE = 8192;
+
+class PooledAllocator {
+ /* We maintain memory alignment to word boundaries by requiring that all
+ allocations be in multiples of the machine wordsize. */
+ /* Size of machine word in bytes. Must be power of 2. */
+ /* Minimum number of bytes requested at a time from the system. Must be
+ * multiple of WORDSIZE. */
+
+ size_t remaining; /* Number of bytes left in current block of storage. */
+ void *base; /* Pointer to base of current block of storage. */
+ void *loc; /* Current location in block to next allocate memory. */
+
+ void internal_init() {
+ remaining = 0;
+ base = NULL;
+ usedMemory = 0;
+ wastedMemory = 0;
+ }
+
+public:
+ size_t usedMemory;
+ size_t wastedMemory;
+
+ /**
+ Default constructor. Initializes a new pool.
+ */
+ PooledAllocator() { internal_init(); }
+
+ /**
+ * Destructor. Frees all the memory allocated in this pool.
+ */
+ ~PooledAllocator() { free_all(); }
+
+ /** Frees all allocated memory chunks */
+ void free_all() {
+ while (base != NULL) {
+ void *prev =
+ *(static_cast(base)); /* Get pointer to prev block. */
+ ::free(base);
+ base = prev;
+ }
+ internal_init();
+ }
+
+ /**
+ * Returns a pointer to a piece of new memory of the given size in bytes
+ * allocated from the pool.
+ */
+ void *malloc(const size_t req_size) {
+ /* Round size up to a multiple of wordsize. The following expression
+ only works for WORDSIZE that is a power of 2, by masking last bits of
+ incremented size to zero.
+ */
+ const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1);
+
+ /* Check whether a new block must be allocated. Note that the first word
+ of a block is reserved for a pointer to the previous block.
+ */
+ if (size > remaining) {
+
+ wastedMemory += remaining;
+
+ /* Allocate new storage. */
+ const size_t blocksize =
+ (size + sizeof(void *) + (WORDSIZE - 1) > BLOCKSIZE)
+ ? size + sizeof(void *) + (WORDSIZE - 1)
+ : BLOCKSIZE;
+
+ // use the standard C malloc to allocate memory
+ void *m = ::malloc(blocksize);
+ if (!m) {
+ fprintf(stderr, "Failed to allocate memory.\n");
+ throw std::bad_alloc();
+ }
+
+ /* Fill first word of new block with pointer to previous block. */
+ static_cast(m)[0] = base;
+ base = m;
+
+ size_t shift = 0;
+ // int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) &
+ // (WORDSIZE-1))) & (WORDSIZE-1);
+
+ remaining = blocksize - sizeof(void *) - shift;
+ loc = (static_cast(m) + sizeof(void *) + shift);
+ }
+ void *rloc = loc;
+ loc = static_cast(loc) + size;
+ remaining -= size;
+
+ usedMemory += size;
+
+ return rloc;
+ }
+
+ /**
+ * Allocates (using this pool) a generic type T.
+ *
+ * Params:
+ * count = number of instances to allocate.
+ * Returns: pointer (of type T*) to memory buffer
+ */
+ template T *allocate(const size_t count = 1) {
+ T *mem = static_cast(this->malloc(sizeof(T) * count));
+ return mem;
+ }
+};
+/** @} */
+
+/** @addtogroup nanoflann_metaprog_grp Auxiliary metaprogramming stuff
+ * @{ */
+
+/** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors
+ * when DIM=-1. Fixed size version for a generic DIM:
+ */
+template struct array_or_vector_selector {
+ typedef std::array container_t;
+};
+/** Dynamic size version */
+template struct array_or_vector_selector<-1, T> {
+ typedef std::vector container_t;
+};
+
+/** @} */
+
+/** kd-tree base-class
+ *
+ * Contains the member functions common to the classes KDTreeSingleIndexAdaptor
+ * and KDTreeSingleIndexDynamicAdaptor_.
+ *
+ * \tparam Derived The name of the class which inherits this class.
+ * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
+ * \tparam Distance The distance metric to use, these are all classes derived
+ * from nanoflann::Metric \tparam DIM Dimensionality of data points (e.g. 3 for
+ * 3D points) \tparam IndexType Will be typically size_t or int
+ */
+
+template
+class KDTreeBaseClass {
+
+public:
+ /** Frees the previously-built index. Automatically called within
+ * buildIndex(). */
+ void freeIndex(Derived &obj) {
+ obj.pool.free_all();
+ obj.root_node = NULL;
+ obj.m_size_at_index_build = 0;
+ }
+
+ typedef typename Distance::ElementType ElementType;
+ typedef typename Distance::DistanceType DistanceType;
+
+ /*--------------------- Internal Data Structures --------------------------*/
+ struct Node {
+ /** Union used because a node can be either a LEAF node or a non-leaf node,
+ * so both data fields are never used simultaneously */
+ union {
+ struct leaf {
+ IndexType left, right; //!< Indices of points in leaf node
+ } lr;
+ struct nonleaf {
+ int divfeat; //!< Dimension used for subdivision.
+ DistanceType divlow, divhigh; //!< The values used for subdivision.
+ } sub;
+ } node_type;
+ Node *child1, *child2; //!< Child nodes (both=NULL mean its a leaf node)
+ };
+
+ typedef Node *NodePtr;
+
+ struct Interval {
+ ElementType low, high;
+ };
+
+ /**
+ * Array of indices to vectors in the dataset.
+ */
+ std::vector vind;
+
+ NodePtr root_node;
+
+ size_t m_leaf_max_size;
+
+ size_t m_size; //!< Number of current points in the dataset
+ size_t m_size_at_index_build; //!< Number of points in the dataset when the
+ //!< index was built
+ int dim; //!< Dimensionality of each data point
+
+ /** Define "BoundingBox" as a fixed-size or variable-size container depending
+ * on "DIM" */
+ typedef
+ typename array_or_vector_selector::container_t BoundingBox;
+
+ /** Define "distance_vector_t" as a fixed-size or variable-size container
+ * depending on "DIM" */
+ typedef typename array_or_vector_selector::container_t
+ distance_vector_t;
+
+ /** The KD-tree used to find neighbours */
+
+ BoundingBox root_bbox;
+
+ /**
+ * Pooled memory allocator.
+ *
+ * Using a pooled memory allocator is more efficient
+ * than allocating memory directly when there is a large
+ * number small of memory allocations.
+ */
+ PooledAllocator pool;
+
+ /** Returns number of points in dataset */
+ size_t size(const Derived &obj) const { return obj.m_size; }
+
+ /** Returns the length of each point in the dataset */
+ size_t veclen(const Derived &obj) {
+ return static_cast(DIM > 0 ? DIM : obj.dim);
+ }
+
+ /// Helper accessor to the dataset points:
+ inline ElementType dataset_get(const Derived &obj, size_t idx,
+ int component) const {
+ return obj.dataset.kdtree_get_pt(idx, component);
+ }
+
+ /**
+ * Computes the inde memory usage
+ * Returns: memory used by the index
+ */
+ size_t usedMemory(Derived &obj) {
+ return obj.pool.usedMemory + obj.pool.wastedMemory +
+ obj.dataset.kdtree_get_point_count() *
+ sizeof(IndexType); // pool memory and vind array memory
+ }
+
+ void computeMinMax(const Derived &obj, IndexType *ind, IndexType count,
+ int element, ElementType &min_elem,
+ ElementType &max_elem) {
+ min_elem = dataset_get(obj, ind[0], element);
+ max_elem = dataset_get(obj, ind[0], element);
+ for (IndexType i = 1; i < count; ++i) {
+ ElementType val = dataset_get(obj, ind[i], element);
+ if (val < min_elem)
+ min_elem = val;
+ if (val > max_elem)
+ max_elem = val;
+ }
+ }
+
+ /**
+ * Create a tree node that subdivides the list of vecs from vind[first]
+ * to vind[last]. The routine is called recursively on each sublist.
+ *
+ * @param left index of the first vector
+ * @param right index of the last vector
+ */
+ NodePtr divideTree(Derived &obj, const IndexType left, const IndexType right,
+ BoundingBox &bbox) {
+ NodePtr node = obj.pool.template allocate(); // allocate memory
+
+ /* If too few exemplars remain, then make this a leaf node. */
+ if ((right - left) <= static_cast(obj.m_leaf_max_size)) {
+ node->child1 = node->child2 = NULL; /* Mark as leaf node. */
+ node->node_type.lr.left = left;
+ node->node_type.lr.right = right;
+
+ // compute bounding-box of leaf points
+ for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
+ bbox[i].low = dataset_get(obj, obj.vind[left], i);
+ bbox[i].high = dataset_get(obj, obj.vind[left], i);
+ }
+ for (IndexType k = left + 1; k < right; ++k) {
+ for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
+ if (bbox[i].low > dataset_get(obj, obj.vind[k], i))
+ bbox[i].low = dataset_get(obj, obj.vind[k], i);
+ if (bbox[i].high < dataset_get(obj, obj.vind[k], i))
+ bbox[i].high = dataset_get(obj, obj.vind[k], i);
+ }
+ }
+ } else {
+ IndexType idx;
+ int cutfeat;
+ DistanceType cutval;
+ middleSplit_(obj, &obj.vind[0] + left, right - left, idx, cutfeat, cutval,
+ bbox);
+
+ node->node_type.sub.divfeat = cutfeat;
+
+ BoundingBox left_bbox(bbox);
+ left_bbox[cutfeat].high = cutval;
+ node->child1 = divideTree(obj, left, left + idx, left_bbox);
+
+ BoundingBox right_bbox(bbox);
+ right_bbox[cutfeat].low = cutval;
+ node->child2 = divideTree(obj, left + idx, right, right_bbox);
+
+ node->node_type.sub.divlow = left_bbox[cutfeat].high;
+ node->node_type.sub.divhigh = right_bbox[cutfeat].low;
+
+ for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
+ bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
+ bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
+ }
+ }
+
+ return node;
+ }
+
+ void middleSplit_(Derived &obj, IndexType *ind, IndexType count,
+ IndexType &index, int &cutfeat, DistanceType &cutval,
+ const BoundingBox &bbox) {
+ const DistanceType EPS = static_cast(0.00001);
+ ElementType max_span = bbox[0].high - bbox[0].low;
+ for (int i = 1; i < (DIM > 0 ? DIM : obj.dim); ++i) {
+ ElementType span = bbox[i].high - bbox[i].low;
+ if (span > max_span) {
+ max_span = span;
+ }
+ }
+ ElementType max_spread = -1;
+ cutfeat = 0;
+ for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
+ ElementType span = bbox[i].high - bbox[i].low;
+ if (span > (1 - EPS) * max_span) {
+ ElementType min_elem, max_elem;
+ computeMinMax(obj, ind, count, i, min_elem, max_elem);
+ ElementType spread = max_elem - min_elem;
+ ;
+ if (spread > max_spread) {
+ cutfeat = i;
+ max_spread = spread;
+ }
+ }
+ }
+ // split in the middle
+ DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2;
+ ElementType min_elem, max_elem;
+ computeMinMax(obj, ind, count, cutfeat, min_elem, max_elem);
+
+ if (split_val < min_elem)
+ cutval = min_elem;
+ else if (split_val > max_elem)
+ cutval = max_elem;
+ else
+ cutval = split_val;
+
+ IndexType lim1, lim2;
+ planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2);
+
+ if (lim1 > count / 2)
+ index = lim1;
+ else if (lim2 < count / 2)
+ index = lim2;
+ else
+ index = count / 2;
+ }
+
+ /**
+ * Subdivide the list of points by a plane perpendicular on axe corresponding
+ * to the 'cutfeat' dimension at 'cutval' position.
+ *
+ * On return:
+ * dataset[ind[0..lim1-1]][cutfeat]cutval
+ */
+ void planeSplit(Derived &obj, IndexType *ind, const IndexType count,
+ int cutfeat, DistanceType &cutval, IndexType &lim1,
+ IndexType &lim2) {
+ /* Move vector indices for left subtree to front of list. */
+ IndexType left = 0;
+ IndexType right = count - 1;
+ for (;;) {
+ while (left <= right && dataset_get(obj, ind[left], cutfeat) < cutval)
+ ++left;
+ while (right && left <= right &&
+ dataset_get(obj, ind[right], cutfeat) >= cutval)
+ --right;
+ if (left > right || !right)
+ break; // "!right" was added to support unsigned Index types
+ std::swap(ind[left], ind[right]);
+ ++left;
+ --right;
+ }
+ /* If either list is empty, it means that all remaining features
+ * are identical. Split in the middle to maintain a balanced tree.
+ */
+ lim1 = left;
+ right = count - 1;
+ for (;;) {
+ while (left <= right && dataset_get(obj, ind[left], cutfeat) <= cutval)
+ ++left;
+ while (right && left <= right &&
+ dataset_get(obj, ind[right], cutfeat) > cutval)
+ --right;
+ if (left > right || !right)
+ break; // "!right" was added to support unsigned Index types
+ std::swap(ind[left], ind[right]);
+ ++left;
+ --right;
+ }
+ lim2 = left;
+ }
+
+ DistanceType computeInitialDistances(const Derived &obj,
+ const ElementType *vec,
+ distance_vector_t &dists) const {
+ assert(vec);
+ DistanceType distsq = DistanceType();
+
+ for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
+ if (vec[i] < obj.root_bbox[i].low) {
+ dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].low, i);
+ distsq += dists[i];
+ }
+ if (vec[i] > obj.root_bbox[i].high) {
+ dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].high, i);
+ distsq += dists[i];
+ }
+ }
+ return distsq;
+ }
+
+ void save_tree(Derived &obj, FILE *stream, NodePtr tree) {
+ save_value(stream, *tree);
+ if (tree->child1 != NULL) {
+ save_tree(obj, stream, tree->child1);
+ }
+ if (tree->child2 != NULL) {
+ save_tree(obj, stream, tree->child2);
+ }
+ }
+
+ void load_tree(Derived &obj, FILE *stream, NodePtr &tree) {
+ tree = obj.pool.template allocate();
+ load_value(stream, *tree);
+ if (tree->child1 != NULL) {
+ load_tree(obj, stream, tree->child1);
+ }
+ if (tree->child2 != NULL) {
+ load_tree(obj, stream, tree->child2);
+ }
+ }
+
+ /** Stores the index in a binary file.
+ * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when
+ * loading the index object it must be constructed associated to the same
+ * source of data points used while building it. See the example:
+ * examples/saveload_example.cpp \sa loadIndex */
+ void saveIndex_(Derived &obj, FILE *stream) {
+ save_value(stream, obj.m_size);
+ save_value(stream, obj.dim);
+ save_value(stream, obj.root_bbox);
+ save_value(stream, obj.m_leaf_max_size);
+ save_value(stream, obj.vind);
+ save_tree(obj, stream, obj.root_node);
+ }
+
+ /** Loads a previous index from a binary file.
+ * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the
+ * index object must be constructed associated to the same source of data
+ * points used while building the index. See the example:
+ * examples/saveload_example.cpp \sa loadIndex */
+ void loadIndex_(Derived &obj, FILE *stream) {
+ load_value(stream, obj.m_size);
+ load_value(stream, obj.dim);
+ load_value(stream, obj.root_bbox);
+ load_value(stream, obj.m_leaf_max_size);
+ load_value(stream, obj.vind);
+ load_tree(obj, stream, obj.root_node);
+ }
+};
+
+/** @addtogroup kdtrees_grp KD-tree classes and adaptors
+ * @{ */
+
+/** kd-tree static index
+ *
+ * Contains the k-d trees and other information for indexing a set of points
+ * for nearest-neighbor matching.
+ *
+ * The class "DatasetAdaptor" must provide the following interface (can be
+ * non-virtual, inlined methods):
+ *
+ * \code
+ * // Must return the number of data poins
+ * inline size_t kdtree_get_point_count() const { ... }
+ *
+ *
+ * // Must return the dim'th component of the idx'th point in the class:
+ * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... }
+ *
+ * // Optional bounding-box computation: return false to default to a standard
+ * bbox computation loop.
+ * // Return true if the BBOX was already computed by the class and returned
+ * in "bb" so it can be avoided to redo it again.
+ * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3
+ * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const
+ * {
+ * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits
+ * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits
+ * ...
+ * return true;
+ * }
+ *
+ * \endcode
+ *
+ * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
+ * \tparam Distance The distance metric to use: nanoflann::metric_L1,
+ * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM
+ * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will
+ * be typically size_t or int
+ */
+template
+class KDTreeSingleIndexAdaptor
+ : public KDTreeBaseClass<
+ KDTreeSingleIndexAdaptor,
+ Distance, DatasetAdaptor, DIM, IndexType> {
+public:
+ /** Deleted copy constructor*/
+ KDTreeSingleIndexAdaptor(
+ const KDTreeSingleIndexAdaptor
+ &) = delete;
+
+ /**
+ * The dataset used by this index
+ */
+ const DatasetAdaptor &dataset; //!< The source of our data
+
+ const KDTreeSingleIndexAdaptorParams index_params;
+
+ Distance distance;
+
+ typedef typename nanoflann::KDTreeBaseClass<
+ nanoflann::KDTreeSingleIndexAdaptor,
+ Distance, DatasetAdaptor, DIM, IndexType>
+ BaseClassRef;
+
+ typedef typename BaseClassRef::ElementType ElementType;
+ typedef typename BaseClassRef::DistanceType DistanceType;
+
+ typedef typename BaseClassRef::Node Node;
+ typedef Node *NodePtr;
+
+ typedef typename BaseClassRef::Interval Interval;
+ /** Define "BoundingBox" as a fixed-size or variable-size container depending
+ * on "DIM" */
+ typedef typename BaseClassRef::BoundingBox BoundingBox;
+
+ /** Define "distance_vector_t" as a fixed-size or variable-size container
+ * depending on "DIM" */
+ typedef typename BaseClassRef::distance_vector_t distance_vector_t;
+
+ /**
+ * KDTree constructor
+ *
+ * Refer to docs in README.md or online in
+ * https://github.com/jlblancoc/nanoflann
+ *
+ * The KD-Tree point dimension (the length of each point in the datase, e.g. 3
+ * for 3D points) is determined by means of:
+ * - The \a DIM template parameter if >0 (highest priority)
+ * - Otherwise, the \a dimensionality parameter of this constructor.
+ *
+ * @param inputData Dataset with the input features
+ * @param params Basically, the maximum leaf node size
+ */
+ KDTreeSingleIndexAdaptor(const int dimensionality,
+ const DatasetAdaptor &inputData,
+ const KDTreeSingleIndexAdaptorParams ¶ms =
+ KDTreeSingleIndexAdaptorParams())
+ : dataset(inputData), index_params(params), distance(inputData) {
+ BaseClassRef::root_node = NULL;
+ BaseClassRef::m_size = dataset.kdtree_get_point_count();
+ BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
+ BaseClassRef::dim = dimensionality;
+ if (DIM > 0)
+ BaseClassRef::dim = DIM;
+ BaseClassRef::m_leaf_max_size = params.leaf_max_size;
+
+ // Create a permutable array of indices to the input vectors.
+ init_vind();
+ }
+
+ /**
+ * Builds the index
+ */
+ void buildIndex() {
+ BaseClassRef::m_size = dataset.kdtree_get_point_count();
+ BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
+ init_vind();
+ this->freeIndex(*this);
+ BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
+ if (BaseClassRef::m_size == 0)
+ return;
+ computeBoundingBox(BaseClassRef::root_bbox);
+ BaseClassRef::root_node =
+ this->divideTree(*this, 0, BaseClassRef::m_size,
+ BaseClassRef::root_bbox); // construct the tree
+ }
+
+ /** \name Query methods
+ * @{ */
+
+ /**
+ * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored
+ * inside the result object.
+ *
+ * Params:
+ * result = the result object in which the indices of the
+ * nearest-neighbors are stored vec = the vector for which to search the
+ * nearest neighbors
+ *
+ * \tparam RESULTSET Should be any ResultSet
+ * \return True if the requested neighbors could be found.
+ * \sa knnSearch, radiusSearch
+ */
+ template
+ bool findNeighbors(RESULTSET &result, const ElementType *vec,
+ const SearchParams &searchParams) const {
+ assert(vec);
+ if (this->size(*this) == 0)
+ return false;
+ if (!BaseClassRef::root_node)
+ throw std::runtime_error(
+ "[nanoflann] findNeighbors() called before building the index.");
+ float epsError = 1 + searchParams.eps;
+
+ distance_vector_t
+ dists; // fixed or variable-sized container (depending on DIM)
+ auto zero = static_cast(0);
+ assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim),
+ zero); // Fill it with zeros.
+ DistanceType distsq = this->computeInitialDistances(*this, vec, dists);
+ searchLevel(result, vec, BaseClassRef::root_node, distsq, dists,
+ epsError); // "count_leaf" parameter removed since was neither
+ // used nor returned to the user.
+ return result.full();
+ }
+
+ /**
+ * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1].
+ * Their indices are stored inside the result object. \sa radiusSearch,
+ * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility
+ * with the original FLANN interface. \return Number `N` of valid points in
+ * the result set. Only the first `N` entries in `out_indices` and
+ * `out_distances_sq` will be valid. Return may be less than `num_closest`
+ * only if the number of elements in the tree is less than `num_closest`.
+ */
+ size_t knnSearch(const ElementType *query_point, const size_t num_closest,
+ IndexType *out_indices, DistanceType *out_distances_sq,
+ const int /* nChecks_IGNORED */ = 10) const {
+ nanoflann::KNNResultSet resultSet(num_closest);
+ resultSet.init(out_indices, out_distances_sq);
+ this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
+ return resultSet.size();
+ }
+
+ /**
+ * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius.
+ * The output is given as a vector of pairs, of which the first element is a
+ * point index and the second the corresponding distance. Previous contents of
+ * \a IndicesDists are cleared.
+ *
+ * If searchParams.sorted==true, the output list is sorted by ascending
+ * distances.
+ *
+ * For a better performance, it is advisable to do a .reserve() on the vector
+ * if you have any wild guess about the number of expected matches.
+ *
+ * \sa knnSearch, findNeighbors, radiusSearchCustomCallback
+ * \return The number of points within the given radius (i.e. indices.size()
+ * or dists.size() )
+ */
+ size_t
+ radiusSearch(const ElementType *query_point, const DistanceType &radius,
+ std::vector> &IndicesDists,
+ const SearchParams &searchParams) const {
+ RadiusResultSet resultSet(radius, IndicesDists);
+ const size_t nFound =
+ radiusSearchCustomCallback(query_point, resultSet, searchParams);
+ if (searchParams.sorted)
+ std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter());
+ return nFound;
+ }
+
+ /**
+ * Just like radiusSearch() but with a custom callback class for each point
+ * found in the radius of the query. See the source of RadiusResultSet<> as a
+ * start point for your own classes. \sa radiusSearch
+ */
+ template
+ size_t radiusSearchCustomCallback(
+ const ElementType *query_point, SEARCH_CALLBACK &resultSet,
+ const SearchParams &searchParams = SearchParams()) const {
+ this->findNeighbors(resultSet, query_point, searchParams);
+ return resultSet.size();
+ }
+
+ /** @} */
+
+public:
+ /** Make sure the auxiliary list \a vind has the same size than the current
+ * dataset, and re-generate if size has changed. */
+ void init_vind() {
+ // Create a permutable array of indices to the input vectors.
+ BaseClassRef::m_size = dataset.kdtree_get_point_count();
+ if (BaseClassRef::vind.size() != BaseClassRef::m_size)
+ BaseClassRef::vind.resize(BaseClassRef::m_size);
+ for (size_t i = 0; i < BaseClassRef::m_size; i++)
+ BaseClassRef::vind[i] = i;
+ }
+
+ void computeBoundingBox(BoundingBox &bbox) {
+ resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim));
+ if (dataset.kdtree_get_bbox(bbox)) {
+ // Done! It was implemented in derived class
+ } else {
+ const size_t N = dataset.kdtree_get_point_count();
+ if (!N)
+ throw std::runtime_error("[nanoflann] computeBoundingBox() called but "
+ "no data points found.");
+ for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {
+ bbox[i].low = bbox[i].high = this->dataset_get(*this, 0, i);
+ }
+ for (size_t k = 1; k < N; ++k) {
+ for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {
+ if (this->dataset_get(*this, k, i) < bbox[i].low)
+ bbox[i].low = this->dataset_get(*this, k, i);
+ if (this->dataset_get(*this, k, i) > bbox[i].high)
+ bbox[i].high = this->dataset_get(*this, k, i);
+ }
+ }
+ }
+ }
+
+ /**
+ * Performs an exact search in the tree starting from a node.
+ * \tparam RESULTSET Should be any ResultSet
+ * \return true if the search should be continued, false if the results are
+ * sufficient
+ */
+ template
+ bool searchLevel(RESULTSET &result_set, const ElementType *vec,
+ const NodePtr node, DistanceType mindistsq,
+ distance_vector_t &dists, const float epsError) const {
+ /* If this is a leaf node, then do check and return. */
+ if ((node->child1 == NULL) && (node->child2 == NULL)) {
+ // count_leaf += (node->lr.right-node->lr.left); // Removed since was
+ // neither used nor returned to the user.
+ DistanceType worst_dist = result_set.worstDist();
+ for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right;
+ ++i) {
+ const IndexType index = BaseClassRef::vind[i]; // reorder... : i;
+ DistanceType dist = distance.evalMetric(
+ vec, index, (DIM > 0 ? DIM : BaseClassRef::dim));
+ if (dist < worst_dist) {
+ if (!result_set.addPoint(dist, BaseClassRef::vind[i])) {
+ // the resultset doesn't want to receive any more points, we're done
+ // searching!
+ return false;
+ }
+ }
+ }
+ return true;
+ }
+
+ /* Which child branch should be taken first? */
+ int idx = node->node_type.sub.divfeat;
+ ElementType val = vec[idx];
+ DistanceType diff1 = val - node->node_type.sub.divlow;
+ DistanceType diff2 = val - node->node_type.sub.divhigh;
+
+ NodePtr bestChild;
+ NodePtr otherChild;
+ DistanceType cut_dist;
+ if ((diff1 + diff2) < 0) {
+ bestChild = node->child1;
+ otherChild = node->child2;
+ cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx);
+ } else {
+ bestChild = node->child2;
+ otherChild = node->child1;
+ cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx);
+ }
+
+ /* Call recursively to search next level down. */
+ if (!searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError)) {
+ // the resultset doesn't want to receive any more points, we're done
+ // searching!
+ return false;
+ }
+
+ DistanceType dst = dists[idx];
+ mindistsq = mindistsq + cut_dist - dst;
+ dists[idx] = cut_dist;
+ if (mindistsq * epsError <= result_set.worstDist()) {
+ if (!searchLevel(result_set, vec, otherChild, mindistsq, dists,
+ epsError)) {
+ // the resultset doesn't want to receive any more points, we're done
+ // searching!
+ return false;
+ }
+ }
+ dists[idx] = dst;
+ return true;
+ }
+
+public:
+ /** Stores the index in a binary file.
+ * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when
+ * loading the index object it must be constructed associated to the same
+ * source of data points used while building it. See the example:
+ * examples/saveload_example.cpp \sa loadIndex */
+ void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); }
+
+ /** Loads a previous index from a binary file.
+ * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the
+ * index object must be constructed associated to the same source of data
+ * points used while building the index. See the example:
+ * examples/saveload_example.cpp \sa loadIndex */
+ void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); }
+
+}; // class KDTree
+
+/** kd-tree dynamic index
+ *
+ * Contains the k-d trees and other information for indexing a set of points
+ * for nearest-neighbor matching.
+ *
+ * The class "DatasetAdaptor" must provide the following interface (can be
+ * non-virtual, inlined methods):
+ *
+ * \code
+ * // Must return the number of data poins
+ * inline size_t kdtree_get_point_count() const { ... }
+ *
+ * // Must return the dim'th component of the idx'th point in the class:
+ * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... }
+ *
+ * // Optional bounding-box computation: return false to default to a standard
+ * bbox computation loop.
+ * // Return true if the BBOX was already computed by the class and returned
+ * in "bb" so it can be avoided to redo it again.
+ * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3
+ * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const
+ * {
+ * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits
+ * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits
+ * ...
+ * return true;
+ * }
+ *
+ * \endcode
+ *
+ * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
+ * \tparam Distance The distance metric to use: nanoflann::metric_L1,
+ * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM
+ * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will
+ * be typically size_t or int
+ */
+template
+class KDTreeSingleIndexDynamicAdaptor_
+ : public KDTreeBaseClass,
+ Distance, DatasetAdaptor, DIM, IndexType> {
+public:
+ /**
+ * The dataset used by this index
+ */
+ const DatasetAdaptor &dataset; //!< The source of our data
+
+ KDTreeSingleIndexAdaptorParams index_params;
+
+ std::vector &treeIndex;
+
+ Distance distance;
+
+ typedef typename nanoflann::KDTreeBaseClass<
+ nanoflann::KDTreeSingleIndexDynamicAdaptor_,
+ Distance, DatasetAdaptor, DIM, IndexType>
+ BaseClassRef;
+
+ typedef typename BaseClassRef::ElementType ElementType;
+ typedef typename BaseClassRef::DistanceType DistanceType;
+
+ typedef typename BaseClassRef::Node Node;
+ typedef Node *NodePtr;
+
+ typedef typename BaseClassRef::Interval Interval;
+ /** Define "BoundingBox" as a fixed-size or variable-size container depending
+ * on "DIM" */
+ typedef typename BaseClassRef::BoundingBox BoundingBox;
+
+ /** Define "distance_vector_t" as a fixed-size or variable-size container
+ * depending on "DIM" */
+ typedef typename BaseClassRef::distance_vector_t distance_vector_t;
+
+ /**
+ * KDTree constructor
+ *
+ * Refer to docs in README.md or online in
+ * https://github.com/jlblancoc/nanoflann
+ *
+ * The KD-Tree point dimension (the length of each point in the datase, e.g. 3
+ * for 3D points) is determined by means of:
+ * - The \a DIM template parameter if >0 (highest priority)
+ * - Otherwise, the \a dimensionality parameter of this constructor.
+ *
+ * @param inputData Dataset with the input features
+ * @param params Basically, the maximum leaf node size
+ */
+ KDTreeSingleIndexDynamicAdaptor_(
+ const int dimensionality, const DatasetAdaptor &inputData,
+ std::vector &treeIndex_,
+ const KDTreeSingleIndexAdaptorParams ¶ms =
+ KDTreeSingleIndexAdaptorParams())
+ : dataset(inputData), index_params(params), treeIndex(treeIndex_),
+ distance(inputData) {
+ BaseClassRef::root_node = NULL;
+ BaseClassRef::m_size = 0;
+ BaseClassRef::m_size_at_index_build = 0;
+ BaseClassRef::dim = dimensionality;
+ if (DIM > 0)
+ BaseClassRef::dim = DIM;
+ BaseClassRef::m_leaf_max_size = params.leaf_max_size;
+ }
+
+ /** Assignment operator definiton */
+ KDTreeSingleIndexDynamicAdaptor_
+ operator=(const KDTreeSingleIndexDynamicAdaptor_ &rhs) {
+ KDTreeSingleIndexDynamicAdaptor_ tmp(rhs);
+ std::swap(BaseClassRef::vind, tmp.BaseClassRef::vind);
+ std::swap(BaseClassRef::m_leaf_max_size, tmp.BaseClassRef::m_leaf_max_size);
+ std::swap(index_params, tmp.index_params);
+ std::swap(treeIndex, tmp.treeIndex);
+ std::swap(BaseClassRef::m_size, tmp.BaseClassRef::m_size);
+ std::swap(BaseClassRef::m_size_at_index_build,
+ tmp.BaseClassRef::m_size_at_index_build);
+ std::swap(BaseClassRef::root_node, tmp.BaseClassRef::root_node);
+ std::swap(BaseClassRef::root_bbox, tmp.BaseClassRef::root_bbox);
+ std::swap(BaseClassRef::pool, tmp.BaseClassRef::pool);
+ return *this;
+ }
+
+ /**
+ * Builds the index
+ */
+ void buildIndex() {
+ BaseClassRef::m_size = BaseClassRef::vind.size();
+ this->freeIndex(*this);
+ BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
+ if (BaseClassRef::m_size == 0)
+ return;
+ computeBoundingBox(BaseClassRef::root_bbox);
+ BaseClassRef::root_node =
+ this->divideTree(*this, 0, BaseClassRef::m_size,
+ BaseClassRef::root_bbox); // construct the tree
+ }
+
+ /** \name Query methods
+ * @{ */
+
+ /**
+ * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored
+ * inside the result object.
+ *
+ * Params:
+ * result = the result object in which the indices of the
+ * nearest-neighbors are stored vec = the vector for which to search the
+ * nearest neighbors
+ *
+ * \tparam RESULTSET Should be any ResultSet
+ * \return True if the requested neighbors could be found.
+ * \sa knnSearch, radiusSearch
+ */
+ template
+ bool findNeighbors(RESULTSET &result, const ElementType *vec,
+ const SearchParams &searchParams) const {
+ assert(vec);
+ if (this->size(*this) == 0)
+ return false;
+ if (!BaseClassRef::root_node)
+ return false;
+ float epsError = 1 + searchParams.eps;
+
+ // fixed or variable-sized container (depending on DIM)
+ distance_vector_t dists;
+ // Fill it with zeros.
+ assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim),
+ static_cast(0));
+ DistanceType distsq = this->computeInitialDistances(*this, vec, dists);
+ searchLevel(result, vec, BaseClassRef::root_node, distsq, dists,
+ epsError); // "count_leaf" parameter removed since was neither
+ // used nor returned to the user.
+ return result.full();
+ }
+
+ /**
+ * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1].
+ * Their indices are stored inside the result object. \sa radiusSearch,
+ * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility
+ * with the original FLANN interface. \return Number `N` of valid points in
+ * the result set. Only the first `N` entries in `out_indices` and
+ * `out_distances_sq` will be valid. Return may be less than `num_closest`
+ * only if the number of elements in the tree is less than `num_closest`.
+ */
+ size_t knnSearch(const ElementType *query_point, const size_t num_closest,
+ IndexType *out_indices, DistanceType *out_distances_sq,
+ const int /* nChecks_IGNORED */ = 10) const {
+ nanoflann::KNNResultSet resultSet(num_closest);
+ resultSet.init(out_indices, out_distances_sq);
+ this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
+ return resultSet.size();
+ }
+
+ /**
+ * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius.
+ * The output is given as a vector of pairs, of which the first element is a
+ * point index and the second the corresponding distance. Previous contents of
+ * \a IndicesDists are cleared.
+ *
+ * If searchParams.sorted==true, the output list is sorted by ascending
+ * distances.
+ *
+ * For a better performance, it is advisable to do a .reserve() on the vector
+ * if you have any wild guess about the number of expected matches.
+ *
+ * \sa knnSearch, findNeighbors, radiusSearchCustomCallback
+ * \return The number of points within the given radius (i.e. indices.size()
+ * or dists.size() )
+ */
+ size_t
+ radiusSearch(const ElementType *query_point, const DistanceType &radius,
+ std::vector> &IndicesDists,
+ const SearchParams &searchParams) const {
+ RadiusResultSet resultSet(radius, IndicesDists);
+ const size_t nFound =
+ radiusSearchCustomCallback(query_point, resultSet, searchParams);
+ if (searchParams.sorted)
+ std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter());
+ return nFound;
+ }
+
+ /**
+ * Just like radiusSearch() but with a custom callback class for each point
+ * found in the radius of the query. See the source of RadiusResultSet<> as a
+ * start point for your own classes. \sa radiusSearch
+ */
+ template
+ size_t radiusSearchCustomCallback(
+ const ElementType *query_point, SEARCH_CALLBACK &resultSet,
+ const SearchParams &searchParams = SearchParams()) const {
+ this->findNeighbors(resultSet, query_point, searchParams);
+ return resultSet.size();
+ }
+
+ /** @} */
+
+public:
+ void computeBoundingBox(BoundingBox &bbox) {
+ resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim));
+
+ if (dataset.kdtree_get_bbox(bbox)) {
+ // Done! It was implemented in derived class
+ } else {
+ const size_t N = BaseClassRef::m_size;
+ if (!N)
+ throw std::runtime_error("[nanoflann] computeBoundingBox() called but "
+ "no data points found.");
+ for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {
+ bbox[i].low = bbox[i].high =
+ this->dataset_get(*this, BaseClassRef::vind[0], i);
+ }
+ for (size_t k = 1; k < N; ++k) {
+ for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {
+ if (this->dataset_get(*this, BaseClassRef::vind[k], i) < bbox[i].low)
+ bbox[i].low = this->dataset_get(*this, BaseClassRef::vind[k], i);
+ if (this->dataset_get(*this, BaseClassRef::vind[k], i) > bbox[i].high)
+ bbox[i].high = this->dataset_get(*this, BaseClassRef::vind[k], i);
+ }
+ }
+ }
+ }
+
+ /**
+ * Performs an exact search in the tree starting from a node.
+ * \tparam RESULTSET Should be any ResultSet
+ */
+ template
+ void searchLevel(RESULTSET &result_set, const ElementType *vec,
+ const NodePtr node, DistanceType mindistsq,
+ distance_vector_t &dists, const float epsError) const {
+ /* If this is a leaf node, then do check and return. */
+ if ((node->child1 == NULL) && (node->child2 == NULL)) {
+ // count_leaf += (node->lr.right-node->lr.left); // Removed since was
+ // neither used nor returned to the user.
+ DistanceType worst_dist = result_set.worstDist();
+ for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right;
+ ++i) {
+ const IndexType index = BaseClassRef::vind[i]; // reorder... : i;
+ if (treeIndex[index] == -1)
+ continue;
+ DistanceType dist = distance.evalMetric(
+ vec, index, (DIM > 0 ? DIM : BaseClassRef::dim));
+ if (dist < worst_dist) {
+ if (!result_set.addPoint(
+ static_cast(dist),
+ static_cast(
+ BaseClassRef::vind[i]))) {
+ // the resultset doesn't want to receive any more points, we're done
+ // searching!
+ return; // false;
+ }
+ }
+ }
+ return;
+ }
+
+ /* Which child branch should be taken first? */
+ int idx = node->node_type.sub.divfeat;
+ ElementType val = vec[idx];
+ DistanceType diff1 = val - node->node_type.sub.divlow;
+ DistanceType diff2 = val - node->node_type.sub.divhigh;
+
+ NodePtr bestChild;
+ NodePtr otherChild;
+ DistanceType cut_dist;
+ if ((diff1 + diff2) < 0) {
+ bestChild = node->child1;
+ otherChild = node->child2;
+ cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx);
+ } else {
+ bestChild = node->child2;
+ otherChild = node->child1;
+ cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx);
+ }
+
+ /* Call recursively to search next level down. */
+ searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError);
+
+ DistanceType dst = dists[idx];
+ mindistsq = mindistsq + cut_dist - dst;
+ dists[idx] = cut_dist;
+ if (mindistsq * epsError <= result_set.worstDist()) {
+ searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError);
+ }
+ dists[idx] = dst;
+ }
+
+public:
+ /** Stores the index in a binary file.
+ * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when
+ * loading the index object it must be constructed associated to the same
+ * source of data points used while building it. See the example:
+ * examples/saveload_example.cpp \sa loadIndex */
+ void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); }
+
+ /** Loads a previous index from a binary file.
+ * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the
+ * index object must be constructed associated to the same source of data
+ * points used while building the index. See the example:
+ * examples/saveload_example.cpp \sa loadIndex */
+ void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); }
+};
+
+/** kd-tree dynaimic index
+ *
+ * class to create multiple static index and merge their results to behave as
+ * single dynamic index as proposed in Logarithmic Approach.
+ *
+ * Example of usage:
+ * examples/dynamic_pointcloud_example.cpp
+ *
+ * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
+ * \tparam Distance The distance metric to use: nanoflann::metric_L1,
+ * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM
+ * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will
+ * be typically size_t or int
+ */
+template
+class KDTreeSingleIndexDynamicAdaptor {
+public:
+ typedef typename Distance::ElementType ElementType;
+ typedef typename Distance::DistanceType DistanceType;
+
+protected:
+ size_t m_leaf_max_size;
+ size_t treeCount;
+ size_t pointCount;
+
+ /**
+ * The dataset used by this index
+ */
+ const DatasetAdaptor &dataset; //!< The source of our data
+
+ std::vector treeIndex; //!< treeIndex[idx] is the index of tree in which
+ //!< point at idx is stored. treeIndex[idx]=-1
+ //!< means that point has been removed.
+
+ KDTreeSingleIndexAdaptorParams index_params;
+
+ int dim; //!< Dimensionality of each data point
+
+ typedef KDTreeSingleIndexDynamicAdaptor_
+ index_container_t;
+ std::vector index;
+
+public:
+ /** Get a const ref to the internal list of indices; the number of indices is
+ * adapted dynamically as the dataset grows in size. */
+ const std::vector &getAllIndices() const { return index; }
+
+private:
+ /** finds position of least significant unset bit */
+ int First0Bit(IndexType num) {
+ int pos = 0;
+ while (num & 1) {
+ num = num >> 1;
+ pos++;
+ }
+ return pos;
+ }
+
+ /** Creates multiple empty trees to handle dynamic support */
+ void init() {
+ typedef KDTreeSingleIndexDynamicAdaptor_
+ my_kd_tree_t;
+ std::vector index_(
+ treeCount, my_kd_tree_t(dim /*dim*/, dataset, treeIndex, index_params));
+ index = index_;
+ }
+
+public:
+ Distance distance;
+
+ /**
+ * KDTree constructor
+ *
+ * Refer to docs in README.md or online in
+ * https://github.com/jlblancoc/nanoflann
+ *
+ * The KD-Tree point dimension (the length of each point in the datase, e.g. 3
+ * for 3D points) is determined by means of:
+ * - The \a DIM template parameter if >0 (highest priority)
+ * - Otherwise, the \a dimensionality parameter of this constructor.
+ *
+ * @param inputData Dataset with the input features
+ * @param params Basically, the maximum leaf node size
+ */
+ KDTreeSingleIndexDynamicAdaptor(const int dimensionality,
+ const DatasetAdaptor &inputData,
+ const KDTreeSingleIndexAdaptorParams ¶ms =
+ KDTreeSingleIndexAdaptorParams(),
+ const size_t maximumPointCount = 1000000000U)
+ : dataset(inputData), index_params(params), distance(inputData) {
+ treeCount = static_cast(std::log2(maximumPointCount));
+ pointCount = 0U;
+ dim = dimensionality;
+ treeIndex.clear();
+ if (DIM > 0)
+ dim = DIM;
+ m_leaf_max_size = params.leaf_max_size;
+ init();
+ const size_t num_initial_points = dataset.kdtree_get_point_count();
+ if (num_initial_points > 0) {
+ addPoints(0, num_initial_points - 1);
+ }
+ }
+
+ /** Deleted copy constructor*/
+ KDTreeSingleIndexDynamicAdaptor(
+ const KDTreeSingleIndexDynamicAdaptor &) = delete;
+
+ /** Add points to the set, Inserts all points from [start, end] */
+ void addPoints(IndexType start, IndexType end) {
+ size_t count = end - start + 1;
+ treeIndex.resize(treeIndex.size() + count);
+ for (IndexType idx = start; idx <= end; idx++) {
+ int pos = First0Bit(pointCount);
+ index[pos].vind.clear();
+ treeIndex[pointCount] = pos;
+ for (int i = 0; i < pos; i++) {
+ for (int j = 0; j < static_cast(index[i].vind.size()); j++) {
+ index[pos].vind.push_back(index[i].vind[j]);
+ if (treeIndex[index[i].vind[j]] != -1)
+ treeIndex[index[i].vind[j]] = pos;
+ }
+ index[i].vind.clear();
+ index[i].freeIndex(index[i]);
+ }
+ index[pos].vind.push_back(idx);
+ index[pos].buildIndex();
+ pointCount++;
+ }
+ }
+
+ /** Remove a point from the set (Lazy Deletion) */
+ void removePoint(size_t idx) {
+ if (idx >= pointCount)
+ return;
+ treeIndex[idx] = -1;
+ }
+
+ /**
+ * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored
+ * inside the result object.
+ *
+ * Params:
+ * result = the result object in which the indices of the
+ * nearest-neighbors are stored vec = the vector for which to search the
+ * nearest neighbors
+ *
+ * \tparam RESULTSET Should be any ResultSet
+ * \return True if the requested neighbors could be found.
+ * \sa knnSearch, radiusSearch
+ */
+ template
+ bool findNeighbors(RESULTSET &result, const ElementType *vec,
+ const SearchParams &searchParams) const {
+ for (size_t i = 0; i < treeCount; i++) {
+ index[i].findNeighbors(result, &vec[0], searchParams);
+ }
+ return result.full();
+ }
+};
+
+/** An L2-metric KD-tree adaptor for working with data directly stored in an
+ * Eigen Matrix, without duplicating the data storage. You can select whether a
+ * row or column in the matrix represents a point in the state space.
+ *
+ * Example of usage:
+ * \code
+ * Eigen::Matrix mat;
+ * // Fill out "mat"...
+ *
+ * typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix >
+ * my_kd_tree_t; const int max_leaf = 10; my_kd_tree_t mat_index(mat, max_leaf
+ * ); mat_index.index->buildIndex(); mat_index.index->... \endcode
+ *
+ * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality
+ * for the points in the data set, allowing more compiler optimizations. \tparam
+ * Distance The distance metric to use: nanoflann::metric_L1,
+ * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam row_major
+ * If set to true the rows of the matrix are used as the points, if set to false
+ * the columns of the matrix are used as the points.
+ */
+template
+struct KDTreeEigenMatrixAdaptor {
+ typedef KDTreeEigenMatrixAdaptor self_t;
+ typedef typename MatrixType::Scalar num_t;
+ typedef typename MatrixType::Index IndexType;
+ typedef
+ typename Distance::template traits::distance_t metric_t;
+ typedef KDTreeSingleIndexAdaptor
+ index_t;
+
+ index_t *index; //! The kd-tree index for the user to call its methods as
+ //! usual with any other FLANN index.
+
+ /// Constructor: takes a const ref to the matrix object with the data points
+ KDTreeEigenMatrixAdaptor(const size_t dimensionality,
+ const std::reference_wrapper &mat,
+ const int leaf_max_size = 10)
+ : m_data_matrix(mat) {
+ const auto dims = row_major ? mat.get().cols() : mat.get().rows();
+ if (size_t(dims) != dimensionality)
+ throw std::runtime_error(
+ "Error: 'dimensionality' must match column count in data matrix");
+ if (DIM > 0 && int(dims) != DIM)
+ throw std::runtime_error(
+ "Data set dimensionality does not match the 'DIM' template argument");
+ index =
+ new index_t(static_cast(dims), *this /* adaptor */,
+ nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size));
+ index->buildIndex();
+ }
+
+public:
+ /** Deleted copy constructor */
+ KDTreeEigenMatrixAdaptor(const self_t &) = delete;
+
+ ~KDTreeEigenMatrixAdaptor() { delete index; }
+
+ const std::reference_wrapper m_data_matrix;
+
+ /** Query for the \a num_closest closest points to a given point (entered as
+ * query_point[0:dim-1]). Note that this is a short-cut method for
+ * index->findNeighbors(). The user can also call index->... methods as
+ * desired. \note nChecks_IGNORED is ignored but kept for compatibility with
+ * the original FLANN interface.
+ */
+ inline void query(const num_t *query_point, const size_t num_closest,
+ IndexType *out_indices, num_t *out_distances_sq,
+ const int /* nChecks_IGNORED */ = 10) const {
+ nanoflann::KNNResultSet resultSet(num_closest);
+ resultSet.init(out_indices, out_distances_sq);
+ index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
+ }
+
+ /** @name Interface expected by KDTreeSingleIndexAdaptor
+ * @{ */
+
+ const self_t &derived() const { return *this; }
+ self_t &derived() { return *this; }
+
+ // Must return the number of data points
+ inline size_t kdtree_get_point_count() const {
+ if(row_major)
+ return m_data_matrix.get().rows();
+ else
+ return m_data_matrix.get().cols();
+ }
+
+ // Returns the dim'th component of the idx'th point in the class:
+ inline num_t kdtree_get_pt(const IndexType idx, size_t dim) const {
+ if(row_major)
+ return m_data_matrix.get().coeff(idx, IndexType(dim));
+ else
+ return m_data_matrix.get().coeff(IndexType(dim), idx);
+ }
+
+ // Optional bounding-box computation: return false to default to a standard
+ // bbox computation loop.
+ // Return true if the BBOX was already computed by the class and returned in
+ // "bb" so it can be avoided to redo it again. Look at bb.size() to find out
+ // the expected dimensionality (e.g. 2 or 3 for point clouds)
+ template bool kdtree_get_bbox(BBOX & /*bb*/) const {
+ return false;
+ }
+
+ /** @} */
+
+}; // end of KDTreeEigenMatrixAdaptor
+ /** @} */
+
+/** @} */ // end of grouping
+} // namespace nanoflann
+
+#endif /* NANOFLANN_HPP_ */
diff --git a/include/tag16h5.h b/include/tag16h5.h
index e4302a8..60804c4 100644
--- a/include/tag16h5.h
+++ b/include/tag16h5.h
@@ -32,7 +32,7 @@
#ifndef _TAG16H5
#define _TAG16H5
-#include "lidar_tag.h"
+#include "lidartag.h"
BipedLab::GrizTagFamily_t *tag16h5_create();
void tag16h5_destroy(BipedLab::GrizTagFamily_t *tf);
diff --git a/include/tag49h14.h b/include/tag49h14.h
index 6c1fa29..6801e3b 100644
--- a/include/tag49h14.h
+++ b/include/tag49h14.h
@@ -31,7 +31,7 @@
#ifndef _TAG49H14
#define _TAG49H14
-#include "lidar_tag.h"
+#include "lidartag.h"
BipedLab::GrizTagFamily_t *tag49h14_create();
void tag49h14_destroy(BipedLab::GrizTagFamily_t *tf);
diff --git a/include/thread_pool.h b/include/thread_pool.h
new file mode 100644
index 0000000..d6feaf1
--- /dev/null
+++ b/include/thread_pool.h
@@ -0,0 +1,98 @@
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace BipedLab {
+class ThreadPool {
+public:
+ThreadPool (int threads) :
+ shutdown_ (false),
+ job_counter_(0) {
+ // Create the specified number of threads
+ threads_.reserve (threads);
+ for (int i = 0; i < threads; ++i)
+ threads_.emplace_back (std::bind (&ThreadPool::threadEntry, this, i));
+}
+
+~ThreadPool () {
+ {
+ // Unblock any threads and tell them to stop
+ std::unique_lock l (lock_);
+
+ shutdown_ = true;
+ condVar_.notify_all();
+ }
+
+ // Wait for all threads to stop
+ std::cerr << "Joining threads" << std::endl;
+ for (auto& thread : threads_)
+ thread.join();
+}
+
+void enqueueTask(std::function func) {
+ // Place a job on the queu and unblock a thread
+ std::unique_lock l (lock_);
+
+ jobs_.emplace (std::move (func));
+ condVar_.notify_one();
+}
+
+void reset_counter() {
+ job_counter_ = 0;
+}
+
+void wait_until_finished(int job_number) {
+ while (1) {
+ if (job_number == job_counter_) {
+ break;
+ }
+ }
+}
+
+
+protected:
+void threadEntry (int i)
+{
+ std::function job;
+
+ while (1)
+ {
+ {
+ std::unique_lock l (lock_);
+
+ while (! shutdown_ && jobs_.empty())
+ condVar_.wait (l);
+
+ if (jobs_.empty ())
+ {
+ // No jobs to do and we are shutting down
+ std::cerr << "Thread " << i << " terminates" << std::endl;
+ return;
+ }
+
+ // std::cerr << "Thread " << i << " does a job" << std::endl;
+ job = std::move (jobs_.front ());
+ jobs_.pop();
+ job_counter_ ++;
+ }
+
+ // Do the job without holding any locks
+ job ();
+ }
+}
+
+std::atomic job_counter_;
+std::mutex lock_;
+std::condition_variable condVar_;
+bool shutdown_;
+std::queue > jobs_;
+std::vector threads_;
+};
+} // BipedLab namespace
diff --git a/include/types.h b/include/types.h
index 64d1a53..6f074c3 100644
--- a/include/types.h
+++ b/include/types.h
@@ -27,6 +27,11 @@
* AUTHOR: Bruce JK Huang (bjhuang@umich.edu)
* WEBSITE: https://www.brucerobot.com/
*/
+#include
+#include
+#include // high_resolution_clock
+#include "nanoflann.hpp"
+
namespace BipedLab {
typedef velodyne_pointcloud::PointXYZIR PointXYZRI;
@@ -64,11 +69,40 @@ namespace BipedLab {
int max;
} MaxMin_t;
+ struct angleComparision {
+ bool operator() (const float &i, const float &j) const {
+ // if (std::abs(i - j) > 0.0017)
+ // return true;
+ // else
+ // return false;
+ // return (std::abs(i - j) > 0.004);
+ //return (std::abs(i - j) > 0.005);
+ // const int i_int = static_cast(i * 1000);
+ // const int j_int = static_cast(j * 1000);
+ // return i_int < j_int;
+
+ float threshold = 0.3;
+ if (std::abs(i - j) < threshold) {
+ return false;
+ } else {
+ return i < j;
+ }
+ }
+ // bool operator() (const pair &lhs, const pair &rhs) const{
+ // return (lhs.second - lhs.first > rhs.second - rhs.first);
+ // }
+ };
+
+
// Structure for LiDAR system
typedef struct LiDARSystem {
std::vector> point_count_table; // point per ring PointCountTable[Scan][ring]
std::vector max_min_table; // max min points in a scan
std::vector