diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..fa4d35aae1851fe33254f5eac5aedd9da83d54ad
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,76 @@
+cmake_minimum_required(VERSION 2.8)
+project(falkolib)
+set(CMAKE_BUILD_TYPE Release)
+add_definitions(-std=c++0x) # Enabling c++11
+
+SET(falkolib_RUNTIME_OUTPUT_DIRECTORY ${falkolib_SOURCE_DIR}/bin CACHE PATH "Target for the binaries")
+SET(falkolib_LIBRARY_OUTPUT_DIRECTORY ${falkolib_SOURCE_DIR}/lib CACHE PATH "Target for the libraries")
+SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${falkolib_LIBRARY_OUTPUT_DIRECTORY})
+SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${falkolib_LIBRARY_OUTPUT_DIRECTORY})
+SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${falkolib_RUNTIME_OUTPUT_DIRECTORY})
+LIST(APPEND CMAKE_MODULE_PATH ${falkolib_SOURCE_DIR}/cmake_modules)
+
+message(STATUS "CMAKE_MODULE_PATH: ${CMAKE_MODULE_PATH}")
+
+find_package(Boost)
+if (${Boost_FOUND})
+ message(STATUS "Boost_INCLUDE_DIRS: ${Boost_INCLUDE_DIRS}")
+ message(STATUS "Boost_LIBRARY_DIRS: ${Boost_LIBRARY_DIRS}")
+ message(STATUS "Boost_LIBRARIES: ${Boost_LIBRARY_DIRS}")
+ include_directories(${Boost_INCLUDE_DIRS})
+ link_directories(${Boost_LIBRARY_DIRS})
+endif(${Boost_FOUND})
+
+find_package(Eigen3 REQUIRED)
+if (${EIGEN3_FOUND})
+ include_directories(${EIGEN3_INCLUDE_DIR})
+ message(STATUS "EIGEN3_INCLUDE_DIR: ${EIGEN3_INCLUDE_DIR}")
+else(${EIGEN3_FOUND})
+ message(WARNING "Cannot find Eigen3 Library")
+endif(${EIGEN3_FOUND})
+
+message(STATUS "include dir: ${falkolib_SOURCE_DIR}/include")
+message(STATUS "include dir: ${falkolib_SOURCE_DIR}/EXTERNAL")
+include_directories(${falkolib_SOURCE_DIR}/include ${falkolib_SOURCE_DIR}/EXTERNAL)
+
+add_library(falkolib
+ src/Common/HoughSpectrum.cpp
+ src/Feature/FALKOExtractor.cpp
+ src/Feature/OCExtractor.cpp
+ src/Feature/CGH.cpp
+ src/Feature/BSC.cpp
+)
+
+add_executable(testKeypointFalko test/testKeypointFalko.cpp test/testData.cpp)
+target_link_libraries(testKeypointFalko falkolib ${Boost_LIBRARIES})
+
+add_executable(testFalkoAHT test/testFalkoAHT.cpp test/testData.cpp)
+target_link_libraries(testFalkoAHT falkolib ${Boost_LIBRARIES})
+
+add_executable(testFalkoCC test/testFalkoCC.cpp test/testData.cpp)
+target_link_libraries(testFalkoCC falkolib ${Boost_LIBRARIES})
+
+add_executable(testKeypointOC test/testKeypointOC.cpp test/testData.cpp)
+target_link_libraries(testKeypointOC falkolib ${Boost_LIBRARIES}) # boost_iostreams boost_system boost_filesystem
+
+
+# Option "make install": copy binaries
+INSTALL(TARGETS falkolib
+ RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin
+ LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib
+ ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib
+)
+
+# Option "make install": copy headers
+FILE(GLOB headers_Common "${CMAKE_CURRENT_SOURCE_DIR}/include/falkolib/Common/*.h")
+FILE(GLOB headers_Feature "${CMAKE_CURRENT_SOURCE_DIR}/include/falkolib/Feature/*.h")
+FILE(GLOB headers_Matching "${CMAKE_CURRENT_SOURCE_DIR}/include/falkolib/Matching/*.h")
+INSTALL(FILES ${headers_Common} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/falkolib/Common)
+INSTALL(FILES ${headers_Feature} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/falkolib/Feature)
+INSTALL(FILES ${headers_Matching} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/falkolib/Matching)
+
+# Option "make install": copy cmake script
+FILE(GLOB cmake_script "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules/falkolibConfig.cmake")
+message(STATUS "cmake_script " ${cmake_script})
+INSTALL(FILES ${cmake_script} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/falkolib/)
+
diff --git a/LICENSE b/COPYING
similarity index 100%
rename from LICENSE
rename to COPYING
diff --git a/EXTERNAL/gnuplot-iostream.h b/EXTERNAL/gnuplot-iostream.h
new file mode 100644
index 0000000000000000000000000000000000000000..813951d0c3b401db9aed2445956a7556dc1c47ca
--- /dev/null
+++ b/EXTERNAL/gnuplot-iostream.h
@@ -0,0 +1,2211 @@
+// vim:foldmethod=marker
+
+/*
+Copyright (c) 2013 Daniel Stahlke (dan@stahlke.org)
+
+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.
+*/
+
+/* A C++ interface to gnuplot.
+ * Web page: http://www.stahlke.org/dan/gnuplot-iostream
+ * Documentation: https://gitorious.org/gnuplot-iostream/pages/Home
+ *
+ * The whole library consists of this monolithic header file, for ease of installation (the
+ * Makefile and *.cc files are only for examples and tests).
+ *
+ * TODO:
+ * What version of boost is currently required?
+ * Callbacks via gnuplot's 'bind' function. This would allow triggering user functions when
+ * keys are pressed in the gnuplot window. However, it would require a PTY reader thread.
+ * Maybe temporary files read in a thread can replace PTY stuff.
+ */
+
+#ifndef GNUPLOT_IOSTREAM_H
+#define GNUPLOT_IOSTREAM_H
+
+// {{{1 Includes and defines
+
+#define GNUPLOT_IOSTREAM_VERSION 2
+
+#ifndef GNUPLOT_ENABLE_CXX11
+# define GNUPLOT_ENABLE_CXX11 (__cplusplus >= 201103)
+#endif
+
+// C system includes
+#include <cstdio>
+#ifdef GNUPLOT_ENABLE_PTY
+# include <termios.h>
+# include <unistd.h>
+# include <pty.h>
+#endif // GNUPLOT_ENABLE_PTY
+
+// C++ system includes
+#include <fstream>
+#include <iostream>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+#include <utility>
+#include <iomanip>
+#include <vector>
+#include <complex>
+#include <cstdlib>
+#include <cmath>
+
+#if GNUPLOT_ENABLE_CXX11
+# include <tuple>
+#endif
+
+#include <boost/iostreams/device/file_descriptor.hpp>
+#include <boost/iostreams/stream.hpp>
+#include <boost/version.hpp>
+#include <boost/utility.hpp>
+#include <boost/tuple/tuple.hpp>
+#include <boost/mpl/bool.hpp>
+// This is the version of boost which has v3 of the filesystem libraries by default.
+#if BOOST_VERSION >= 104600
+# define GNUPLOT_USE_TMPFILE
+# include <boost/filesystem.hpp>
+#endif // BOOST_VERSION
+
+// This is used because VS2008 doesn't have stdint.h.
+#include <boost/cstdint.hpp>
+
+// Note: this is here for reverse compatibility. The new way to enable blitz support is to
+// just include the gnuplot-iostream.h header after you include the blitz header (likewise for
+// armadillo).
+#ifdef GNUPLOT_ENABLE_BLITZ
+# include <blitz/array.h>
+#endif
+
+#ifdef BOOST_STATIC_ASSERT_MSG
+# define GNUPLOT_STATIC_ASSERT_MSG(cond, msg) BOOST_STATIC_ASSERT_MSG((cond), msg)
+#else
+# define GNUPLOT_STATIC_ASSERT_MSG(cond, msg) BOOST_STATIC_ASSERT((cond))
+#endif
+
+// If this is defined, warn about use of deprecated functions.
+#ifdef GNUPLOT_DEPRECATE_WARN
+# ifdef __GNUC__
+# define GNUPLOT_DEPRECATE(msg) __attribute__ ((deprecated(msg)))
+# elif defined(_MSC_VER)
+# define GNUPLOT_DEPRECATE(msg) __declspec(deprecated(msg))
+# else
+# define GNUPLOT_DEPRECATE(msg)
+# endif
+#else
+# define GNUPLOT_DEPRECATE(msg)
+#endif
+
+// Patch for Windows by Damien Loison
+#ifdef _WIN32
+# include <windows.h>
+# define GNUPLOT_PCLOSE _pclose
+# define GNUPLOT_POPEN _popen
+# define GNUPLOT_FILENO _fileno
+#else
+# define GNUPLOT_PCLOSE pclose
+# define GNUPLOT_POPEN popen
+# define GNUPLOT_FILENO fileno
+#endif
+
+#ifdef _WIN32
+# define GNUPLOT_ISNAN _isnan
+#else
+// cppreference.com says std::isnan is only for C++11. However, this seems to work on Linux
+// and I am assuming that if isnan exists in math.h then std::isnan exists in cmath.
+# define GNUPLOT_ISNAN std::isnan
+#endif
+
+// MSVC gives a warning saying that fopen and getenv are not secure. But they are secure.
+// Unfortunately their replacement functions are not simple drop-in replacements. The best
+// solution is to just temporarily disable this warning whenever fopen or getenv is used.
+// http://stackoverflow.com/a/4805353/1048959
+#if defined(_MSC_VER) && _MSC_VER >= 1400
+# define GNUPLOT_MSVC_WARNING_4996_PUSH \
+ __pragma(warning(push)) \
+ __pragma(warning(disable:4996))
+# define GNUPLOT_MSVC_WARNING_4996_POP \
+ __pragma(warning(pop))
+#else
+# define GNUPLOT_MSVC_WARNING_4996_PUSH
+# define GNUPLOT_MSVC_WARNING_4996_POP
+#endif
+
+#ifndef GNUPLOT_DEFAULT_COMMAND
+#ifdef _WIN32
+// "pgnuplot" is considered deprecated according to the Internet. It may be faster. It
+// doesn't seem to handle binary data though.
+//# define GNUPLOT_DEFAULT_COMMAND "pgnuplot -persist"
+// On Windows, gnuplot echos commands to stderr. So we forward its stderr to the bit bucket.
+// Unfortunately, this means you will miss out on legitimate error messages.
+# define GNUPLOT_DEFAULT_COMMAND "gnuplot -persist 2> NUL"
+#else
+# define GNUPLOT_DEFAULT_COMMAND "gnuplot -persist"
+#endif
+#endif
+
+// }}}1
+
+namespace gnuplotio {
+
+// {{{1 Basic traits helpers
+//
+// The mechanisms constructed in this section enable us to detect what sort of datatype has
+// been passed to a function.
+
+// This can be specialized as needed, in order to not use the STL interfaces for specific
+// classes.
+template <typename T>
+struct dont_treat_as_stl_container {
+ typedef boost::mpl::bool_<false> type;
+};
+
+BOOST_MPL_HAS_XXX_TRAIT_DEF(value_type)
+BOOST_MPL_HAS_XXX_TRAIT_DEF(const_iterator)
+
+template <typename T>
+struct is_like_stl_container {
+ typedef boost::mpl::and_<
+ typename has_value_type<T>::type,
+ typename has_const_iterator<T>::type,
+ boost::mpl::not_<dont_treat_as_stl_container<T> >
+ > type;
+ static const bool value = type::value;
+};
+
+template <typename T>
+struct is_boost_tuple_nulltype {
+ static const bool value = false;
+ typedef boost::mpl::bool_<value> type;
+};
+
+template <>
+struct is_boost_tuple_nulltype<boost::tuples::null_type> {
+ static const bool value = true;
+ typedef boost::mpl::bool_<value> type;
+};
+
+BOOST_MPL_HAS_XXX_TRAIT_DEF(head_type)
+BOOST_MPL_HAS_XXX_TRAIT_DEF(tail_type)
+
+template <typename T>
+struct is_boost_tuple {
+ typedef boost::mpl::and_<
+ typename has_head_type<T>::type,
+ typename has_tail_type<T>::type
+ > type;
+ static const bool value = type::value;
+};
+
+// More fine-grained, but doesn't compile!
+//template <typename T>
+//struct is_boost_tuple {
+// typedef boost::mpl::and_<
+// typename boost::is_class<T>::type,
+// typename boost::mpl::and_<
+// typename has_head_type<T>::type,
+// typename boost::mpl::and_<
+// typename has_tail_type<T>::type,
+// typename boost::mpl::or_<
+// typename is_boost_tuple_nulltype<typename T::tail_type>::type,
+// typename is_boost_tuple<typename T::tail_type>::type
+// >::type
+// >::type
+// >::type
+// > type;
+//};
+//
+//template <>
+//struct is_boost_tuple<boost::tuples::null_type> {
+// typedef boost::mpl::bool_<false> type;
+//};
+
+// }}}1
+
+// {{{1 Tmpfile helper class
+#ifdef GNUPLOT_USE_TMPFILE
+// RAII temporary file. File is removed when this object goes out of scope.
+class GnuplotTmpfile {
+public:
+ GnuplotTmpfile() :
+ file(boost::filesystem::unique_path(
+ boost::filesystem::temp_directory_path() /
+ "tmp-gnuplot-%%%%-%%%%-%%%%-%%%%"))
+ { }
+
+private:
+ // noncopyable
+ GnuplotTmpfile(const GnuplotTmpfile &);
+ const GnuplotTmpfile& operator=(const GnuplotTmpfile &);
+
+public:
+ ~GnuplotTmpfile() {
+ // it is never good to throw exceptions from a destructor
+ try {
+ remove(file);
+ } catch(const std::exception &) {
+ std::cerr << "Failed to remove temporary file " << file << std::endl;
+ }
+ }
+
+public:
+ boost::filesystem::path file;
+};
+#endif // GNUPLOT_USE_TMPFILE
+// }}}1
+
+// {{{1 Feedback helper classes
+//
+// Used for reading stuff sent from gnuplot via gnuplot's "print" function.
+//
+// For example, this is used for capturing mouse clicks in the gnuplot window. There are two
+// implementations, only the first of which is complete. The first implementation allocates a
+// PTY (pseudo terminal) which is written to by gnuplot and read by us. This only works in
+// Linux. The second implementation creates a temporary file which is written to by gnuplot
+// and read by us. However, this doesn't currently work since fscanf doesn't block. It would
+// be possible to get this working using a more complicated mechanism (select or threads) but I
+// haven't had the need for it.
+
+class GnuplotFeedback {
+public:
+ GnuplotFeedback() { }
+ virtual ~GnuplotFeedback() { }
+ virtual std::string filename() const = 0;
+ virtual FILE *handle() const = 0;
+
+private:
+ // noncopyable
+ GnuplotFeedback(const GnuplotFeedback &);
+ const GnuplotFeedback& operator=(const GnuplotFeedback &);
+};
+
+#ifdef GNUPLOT_ENABLE_PTY
+#define GNUPLOT_ENABLE_FEEDBACK
+class GnuplotFeedbackPty : public GnuplotFeedback {
+public:
+ explicit GnuplotFeedbackPty(bool debug_messages) :
+ pty_fn(),
+ pty_fh(NULL),
+ master_fd(-1),
+ slave_fd(-1)
+ {
+ // adapted from http://www.gnuplot.info/files/gpReadMouseTest.c
+ if(0 > openpty(&master_fd, &slave_fd, NULL, NULL, NULL)) {
+ perror("openpty");
+ throw std::runtime_error("openpty failed");
+ }
+ char pty_fn_buf[1024];
+ if(ttyname_r(slave_fd, pty_fn_buf, 1024)) {
+ perror("ttyname_r");
+ throw std::runtime_error("ttyname failed");
+ }
+ pty_fn = std::string(pty_fn_buf);
+ if(debug_messages) {
+ std::cerr << "feedback_fn=" << pty_fn << std::endl;
+ }
+
+ // disable echo
+ struct termios tios;
+ if(tcgetattr(slave_fd, &tios) < 0) {
+ perror("tcgetattr");
+ throw std::runtime_error("tcgetattr failed");
+ }
+ tios.c_lflag &= ~(ECHO | ECHONL);
+ if(tcsetattr(slave_fd, TCSAFLUSH, &tios) < 0) {
+ perror("tcsetattr");
+ throw std::runtime_error("tcsetattr failed");
+ }
+
+ pty_fh = fdopen(master_fd, "r");
+ if(!pty_fh) {
+ throw std::runtime_error("fdopen failed");
+ }
+ }
+
+private:
+ // noncopyable
+ GnuplotFeedbackPty(const GnuplotFeedbackPty &);
+ const GnuplotFeedbackPty& operator=(const GnuplotFeedbackPty &);
+
+public:
+ ~GnuplotFeedbackPty() {
+ if(pty_fh) fclose(pty_fh);
+ if(master_fd > 0) ::close(master_fd);
+ if(slave_fd > 0) ::close(slave_fd);
+ }
+
+ std::string filename() const {
+ return pty_fn;
+ }
+
+ FILE *handle() const {
+ return pty_fh;
+ }
+
+private:
+ std::string pty_fn;
+ FILE *pty_fh;
+ int master_fd, slave_fd;
+};
+//#elif defined GNUPLOT_USE_TMPFILE
+//// Currently this doesn't work since fscanf doesn't block (need something like "tail -f")
+//#define GNUPLOT_ENABLE_FEEDBACK
+//class GnuplotFeedbackTmpfile : public GnuplotFeedback {
+//public:
+// explicit GnuplotFeedbackTmpfile(bool debug_messages) :
+// tmp_file(),
+// fh(NULL)
+// {
+// if(debug_messages) {
+// std::cerr << "feedback_fn=" << filename() << std::endl;
+// }
+// GNUPLOT_MSVC_WARNING_4996_PUSH
+// fh = std::fopen(filename().c_str(), "a");
+// GNUPLOT_MSVC_WARNING_4996_POP
+// }
+//
+// ~GnuplotFeedbackTmpfile() {
+// fclose(fh);
+// }
+//
+//private:
+// // noncopyable
+// GnuplotFeedbackTmpfile(const GnuplotFeedbackTmpfile &);
+// const GnuplotFeedbackTmpfile& operator=(const GnuplotFeedbackTmpfile &);
+//
+//public:
+// std::string filename() const {
+// return tmp_file.file.string();
+// }
+//
+// FILE *handle() const {
+// return fh;
+// }
+//
+//private:
+// GnuplotTmpfile tmp_file;
+// FILE *fh;
+//};
+#endif // GNUPLOT_ENABLE_PTY, GNUPLOT_USE_TMPFILE
+// }}}1
+
+// {{{1 Traits and printers for entry datatypes
+//
+// This section contains the mechanisms for sending scalar and tuple data to gnuplot. Pairs
+// and tuples are sent by appealing to the senders defined for their component scalar types.
+// Senders for arrays are defined in a later section.
+//
+// There are three classes which need to be specialized for each supported datatype:
+// 1. TextSender to send data as text. The default is to just send using the ostream's `<<`
+// operator.
+// 2. BinarySender to send data as binary, in a format which gnuplot can understand. There is
+// no default implementation (unimplemented types raise a compile time error), however
+// inheriting from FlatBinarySender will send the data literally as it is stored in memory.
+// This suffices for most of the standard built-in types (e.g. uint32_t or double).
+// 3. BinfmtSender sends a description of the data format to gnuplot (e.g. `%uint32`). Type
+// `show datafile binary datasizes` in gnuplot to see a list of supported formats.
+
+// {{{2 Basic entry datatypes
+
+// Default TextSender, sends data using `<<` operator.
+template <typename T, typename Enable=void>
+struct TextSender {
+ static void send(std::ostream &stream, const T &v) {
+ stream << v;
+ }
+};
+
+// Default BinarySender, raises a compile time error.
+template <typename T, typename Enable=void>
+struct BinarySender {
+ GNUPLOT_STATIC_ASSERT_MSG((sizeof(T) == 0), "BinarySender class not specialized for this type");
+
+ // This is here to avoid further compilation errors, beyond what the assert prints.
+ static void send(std::ostream &stream, const T &v);
+};
+
+// This is a BinarySender implementation that just sends directly from memory. Data types
+// which can be sent this way can have their BinarySender specialization inherit from this.
+template <typename T>
+struct FlatBinarySender {
+ static void send(std::ostream &stream, const T &v) {
+ stream.write(reinterpret_cast<const char *>(&v), sizeof(T));
+ }
+};
+
+// Default BinfmtSender, raises a compile time error.
+template <typename T, typename Enable=void>
+struct BinfmtSender {
+ GNUPLOT_STATIC_ASSERT_MSG((sizeof(T) == 0), "BinfmtSender class not specialized for this type");
+
+ // This is here to avoid further compilation errors, beyond what the assert prints.
+ static void send(std::ostream &);
+};
+
+// BinfmtSender implementations for basic data types supported by gnuplot.
+// Types from boost/cstdint.hpp are used because VS2008 doesn't have stdint.h.
+template<> struct BinfmtSender< float> { static void send(std::ostream &stream) { stream << "%float"; } };
+template<> struct BinfmtSender<double> { static void send(std::ostream &stream) { stream << "%double"; } };
+template<> struct BinfmtSender<boost:: int8_t> { static void send(std::ostream &stream) { stream << "%int8"; } };
+template<> struct BinfmtSender<boost:: uint8_t> { static void send(std::ostream &stream) { stream << "%uint8"; } };
+template<> struct BinfmtSender<boost:: int16_t> { static void send(std::ostream &stream) { stream << "%int16"; } };
+template<> struct BinfmtSender<boost::uint16_t> { static void send(std::ostream &stream) { stream << "%uint16"; } };
+template<> struct BinfmtSender<boost:: int32_t> { static void send(std::ostream &stream) { stream << "%int32"; } };
+template<> struct BinfmtSender<boost::uint32_t> { static void send(std::ostream &stream) { stream << "%uint32"; } };
+template<> struct BinfmtSender<boost:: int64_t> { static void send(std::ostream &stream) { stream << "%int64"; } };
+template<> struct BinfmtSender<boost::uint64_t> { static void send(std::ostream &stream) { stream << "%uint64"; } };
+
+// BinarySender implementations for basic data types supported by gnuplot. These types can
+// just be sent as stored in memory, so all these senders inherit from FlatBinarySender.
+template<> struct BinarySender< float> : public FlatBinarySender< float> { };
+template<> struct BinarySender<double> : public FlatBinarySender<double> { };
+template<> struct BinarySender<boost:: int8_t> : public FlatBinarySender<boost:: int8_t> { };
+template<> struct BinarySender<boost:: uint8_t> : public FlatBinarySender<boost:: uint8_t> { };
+template<> struct BinarySender<boost:: int16_t> : public FlatBinarySender<boost:: int16_t> { };
+template<> struct BinarySender<boost::uint16_t> : public FlatBinarySender<boost::uint16_t> { };
+template<> struct BinarySender<boost:: int32_t> : public FlatBinarySender<boost:: int32_t> { };
+template<> struct BinarySender<boost::uint32_t> : public FlatBinarySender<boost::uint32_t> { };
+template<> struct BinarySender<boost:: int64_t> : public FlatBinarySender<boost:: int64_t> { };
+template<> struct BinarySender<boost::uint64_t> : public FlatBinarySender<boost::uint64_t> { };
+
+// Make char types print as integers, not as characters.
+template <typename T>
+struct CastIntTextSender {
+ static void send(std::ostream &stream, const T &v) {
+ stream << int(v);
+ }
+};
+template<> struct TextSender< char> : public CastIntTextSender< char> { };
+template<> struct TextSender< signed char> : public CastIntTextSender< signed char> { };
+template<> struct TextSender< unsigned char> : public CastIntTextSender< unsigned char> { };
+
+// Make sure that the same not-a-number string is printed on all platforms.
+template <typename T>
+struct FloatTextSender {
+ static void send(std::ostream &stream, const T &v) {
+ if(GNUPLOT_ISNAN(v)) { stream << "nan"; } else { stream << v; }
+ }
+};
+template<> struct TextSender< float> : FloatTextSender< float> { };
+template<> struct TextSender< double> : FloatTextSender< double> { };
+template<> struct TextSender<long double> : FloatTextSender<long double> { };
+
+// }}}2
+
+// {{{2 std::pair support
+
+template <typename T, typename U>
+struct TextSender<std::pair<T, U> > {
+ static void send(std::ostream &stream, const std::pair<T, U> &v) {
+ TextSender<T>::send(stream, v.first);
+ stream << " ";
+ TextSender<U>::send(stream, v.second);
+ }
+};
+
+template <typename T, typename U>
+struct BinfmtSender<std::pair<T, U> > {
+ static void send(std::ostream &stream) {
+ BinfmtSender<T>::send(stream);
+ BinfmtSender<U>::send(stream);
+ }
+};
+
+template <typename T, typename U>
+struct BinarySender<std::pair<T, U> > {
+ static void send(std::ostream &stream, const std::pair<T, U> &v) {
+ BinarySender<T>::send(stream, v.first);
+ BinarySender<U>::send(stream, v.second);
+ }
+};
+
+// }}}2
+
+// {{{2 std::complex support
+
+template <typename T>
+struct TextSender<std::complex<T> > {
+ static void send(std::ostream &stream, const std::complex<T> &v) {
+ TextSender<T>::send(stream, v.real());
+ stream << " ";
+ TextSender<T>::send(stream, v.imag());
+ }
+};
+
+template <typename T>
+struct BinfmtSender<std::complex<T> > {
+ static void send(std::ostream &stream) {
+ BinfmtSender<T>::send(stream);
+ BinfmtSender<T>::send(stream);
+ }
+};
+
+template <typename T>
+struct BinarySender<std::complex<T> > {
+ static void send(std::ostream &stream, const std::complex<T> &v) {
+ BinarySender<T>::send(stream, v.real());
+ BinarySender<T>::send(stream, v.imag());
+ }
+};
+
+// }}}2
+
+// {{{2 boost::tuple support
+
+template <typename T>
+struct TextSender<T,
+ typename boost::enable_if<
+ boost::mpl::and_<
+ is_boost_tuple<T>,
+ boost::mpl::not_<is_boost_tuple_nulltype<typename T::tail_type> >
+ >
+ >::type
+> {
+ static void send(std::ostream &stream, const T &v) {
+ TextSender<typename T::head_type>::send(stream, v.get_head());
+ stream << " ";
+ TextSender<typename T::tail_type>::send(stream, v.get_tail());
+ }
+};
+
+template <typename T>
+struct TextSender<T,
+ typename boost::enable_if<
+ boost::mpl::and_<
+ is_boost_tuple<T>,
+ is_boost_tuple_nulltype<typename T::tail_type>
+ >
+ >::type
+> {
+ static void send(std::ostream &stream, const T &v) {
+ TextSender<typename T::head_type>::send(stream, v.get_head());
+ }
+};
+
+template <typename T>
+struct BinfmtSender<T,
+ typename boost::enable_if<
+ boost::mpl::and_<
+ is_boost_tuple<T>,
+ boost::mpl::not_<is_boost_tuple_nulltype<typename T::tail_type> >
+ >
+ >::type
+> {
+ static void send(std::ostream &stream) {
+ BinfmtSender<typename T::head_type>::send(stream);
+ stream << " ";
+ BinfmtSender<typename T::tail_type>::send(stream);
+ }
+};
+
+template <typename T>
+struct BinfmtSender<T,
+ typename boost::enable_if<
+ boost::mpl::and_<
+ is_boost_tuple<T>,
+ is_boost_tuple_nulltype<typename T::tail_type>
+ >
+ >::type
+> {
+ static void send(std::ostream &stream) {
+ BinfmtSender<typename T::head_type>::send(stream);
+ }
+};
+
+template <typename T>
+struct BinarySender<T,
+ typename boost::enable_if<
+ boost::mpl::and_<
+ is_boost_tuple<T>,
+ boost::mpl::not_<is_boost_tuple_nulltype<typename T::tail_type> >
+ >
+ >::type
+> {
+ static void send(std::ostream &stream, const T &v) {
+ BinarySender<typename T::head_type>::send(stream, v.get_head());
+ BinarySender<typename T::tail_type>::send(stream, v.get_tail());
+ }
+};
+
+template <typename T>
+struct BinarySender<T,
+ typename boost::enable_if<
+ boost::mpl::and_<
+ is_boost_tuple<T>,
+ is_boost_tuple_nulltype<typename T::tail_type>
+ >
+ >::type
+> {
+ static void send(std::ostream &stream, const T &v) {
+ BinarySender<typename T::head_type>::send(stream, v.get_head());
+ }
+};
+
+// }}}2
+
+// {{{2 std::tuple support
+
+#if GNUPLOT_ENABLE_CXX11
+
+// http://stackoverflow.com/questions/6245735/pretty-print-stdtuple
+
+template<std::size_t> struct int_{}; // compile-time counter
+
+template <typename Tuple, std::size_t I>
+void std_tuple_formatcode_helper(std::ostream &stream, const Tuple *, int_<I>) {
+ std_tuple_formatcode_helper(stream, (const Tuple *)(0), int_<I-1>());
+ stream << " ";
+ BinfmtSender<typename std::tuple_element<I, Tuple>::type>::send(stream);
+}
+
+template <typename Tuple>
+void std_tuple_formatcode_helper(std::ostream &stream, const Tuple *, int_<0>) {
+ BinfmtSender<typename std::tuple_element<0, Tuple>::type>::send(stream);
+}
+
+template <typename... Args>
+struct BinfmtSender<std::tuple<Args...> > {
+ typedef typename std::tuple<Args...> Tuple;
+
+ static void send(std::ostream &stream) {
+ std_tuple_formatcode_helper(stream, (const Tuple *)(0), int_<sizeof...(Args)-1>());
+ }
+};
+
+template <typename Tuple, std::size_t I>
+void std_tuple_textsend_helper(std::ostream &stream, const Tuple &v, int_<I>) {
+ std_tuple_textsend_helper(stream, v, int_<I-1>());
+ stream << " ";
+ TextSender<typename std::tuple_element<I, Tuple>::type>::send(stream, std::get<I>(v));
+}
+
+template <typename Tuple>
+void std_tuple_textsend_helper(std::ostream &stream, const Tuple &v, int_<0>) {
+ TextSender<typename std::tuple_element<0, Tuple>::type>::send(stream, std::get<0>(v));
+}
+
+template <typename... Args>
+struct TextSender<std::tuple<Args...> > {
+ typedef typename std::tuple<Args...> Tuple;
+
+ static void send(std::ostream &stream, const Tuple &v) {
+ std_tuple_textsend_helper(stream, v, int_<sizeof...(Args)-1>());
+ }
+};
+
+template <typename Tuple, std::size_t I>
+void std_tuple_binsend_helper(std::ostream &stream, const Tuple &v, int_<I>) {
+ std_tuple_binsend_helper(stream, v, int_<I-1>());
+ BinarySender<typename std::tuple_element<I, Tuple>::type>::send(stream, std::get<I>(v));
+}
+
+template <typename Tuple>
+void std_tuple_binsend_helper(std::ostream &stream, const Tuple &v, int_<0>) {
+ BinarySender<typename std::tuple_element<0, Tuple>::type>::send(stream, std::get<0>(v));
+}
+
+template <typename... Args>
+struct BinarySender<std::tuple<Args...> > {
+ typedef typename std::tuple<Args...> Tuple;
+
+ static void send(std::ostream &stream, const Tuple &v) {
+ std_tuple_binsend_helper(stream, v, int_<sizeof...(Args)-1>());
+ }
+};
+
+#endif // GNUPLOT_ENABLE_CXX11
+
+// }}}2
+
+// }}}1
+
+// {{{1 ArrayTraits and Range classes
+//
+// This section handles sending of array data to gnuplot. It is rather complicated because of
+// the diversity of storage schemes supported. For example, it treats a
+// `std::pair<std::vector<T>, std::vector<U>>` in the same way as a
+// `std::vector<std::pair<T, U>>`, iterating through the two arrays in lockstep, and sending
+// pairs <T,U> to gnuplot as columns. In fact, any nested combination of pairs, tuples, STL
+// containers, Blitz arrays, and Armadillo arrays is supported (with the caveat that, for
+// instance, Blitz arrays should never be put into an STL container or you will suffer
+// unpredictable results due the way Blitz handles assignment). Nested containers are
+// considered to be multidimensional arrays. Although gnuplot only supports 1D and 2D arrays,
+// our module is in principle not limited.
+//
+// The ArrayTraits class is specialized for every supported array or container type (the
+// default, unspecialized, version of ArrayTraits exists only to tell you that something is
+// *not* a container, via the is_container flag). ArrayTraits tells you the depth of a nested
+// (or multidimensional) container, as well as the value type, and provides a specialized
+// sort of iterator (a.k.a. "range"). Ranges are sort of like STL iterators, except that they
+// have built-in knowledge of the end condition so you don't have to carry around both a
+// begin() and an end() iterator like in STL.
+//
+// As an example of how this works, consider a std::pair of std::vectors. Ultimately this gets
+// sent to gnuplot as two columns, so the two vectors need to be iterated in lockstep.
+// The `value_type` of `std::pair<std::vector<T>, std::vector<U>>` is then `std::pair<T, U>`
+// and this is what deferencing the range (iterator) gives. Internally, this range is built
+// out of a pair of ranges (PairOfRange class), the `inc()` (advance to next element)
+// method calls `inc()` on each of the children, and `deref()` calls `deref()` on each child
+// and combines the results to return a `std::pair`. Tuples are handled as nested pairs.
+//
+// In addition to PairOfRange, there is also a VecOfRange class that can be used to treat the
+// outermost part of a nested container as if it were a tuple. Since tuples are printed as
+// columns, this is like treating a multidimensional array as if it were column-major. A
+// VecOfRange is obtained by calling `get_columns_range`. This is used by, for instance,
+// `send1d_colmajor`. The implementation is similar to that of PairOfRange.
+//
+// The range, accessed via `ArrayTraits<T>::get_range`, will be of a different class depending
+// on T, and this is defined by the ArrayTraits specialization for T. It will always have
+// methods `inc()` to advance to the next element and `is_end()` for checking whether one has
+// advanced past the final element. For nested containers, `deref_subiter()` returns a range
+// iterator for the next nesting level. When at the innermost level of nesting, `deref()`
+// returns the value of the entry the iterator points to (a scalar, pair, or tuple).
+// Only one of `deref()` or `deref_subiter()` will be available, depending on whether there are
+// deeper levels of nesting. The typedefs `value_type` and `subiter_type` tell the return
+// types of these two methods.
+//
+// Support for standard C++ and boost containers and tuples of containers is provided in this
+// section. Support for third party packages like Blitz and Armadillo is in a later section.
+
+// {{{2 ArrayTraits generic class and defaults
+
+// Error messages involving this stem from treating something that was not a container as if it
+// was. This is only here to allow compiliation without errors in normal cases.
+struct Error_WasNotContainer {
+ // This is just here to make VC++ happy.
+ // https://connect.microsoft.com/VisualStudio/feedback/details/777612/class-template-specialization-that-compiles-in-g-but-not-visual-c
+ typedef void subiter_type;
+};
+
+// Error messages involving this stem from calling deref instead of deref_subiter for a nested
+// container.
+struct Error_InappropriateDeref { };
+
+// The unspecialized version of this class gives traits for things that are *not* arrays.
+template <typename T, typename Enable=void>
+class ArrayTraits {
+public:
+ // The value type of elements after all levels of nested containers have been dereferenced.
+ typedef Error_WasNotContainer value_type;
+ // The type of the range (a.k.a. iterator) that `get_range()` returns.
+ typedef Error_WasNotContainer range_type;
+ // Tells whether T is in fact a container type.
+ static const bool is_container = false;
+ // This flag supports the legacy behavior of automatically guessing whether the data should
+ // be treated as column major. This guessing happens when `send()` is called rather than
+ // `send1d()` or `send2d()`. This is deprecated, but is still supported for reverse
+ // compatibility.
+ static const bool allow_auto_unwrap = false;
+ // The number of levels of nesting, or the dimension of multidimensional arrays.
+ static const size_t depth = 0;
+
+ // Returns the range (iterator) for an array.
+ static range_type get_range(const T &) {
+ GNUPLOT_STATIC_ASSERT_MSG((sizeof(T)==0), "argument was not a container");
+ throw std::logic_error("static assert should have been triggered by this point");
+ }
+};
+
+// Most specializations of ArrayTraits should inherit from this (with V set to the value type).
+// It sets some default values.
+template <typename V>
+class ArrayTraitsDefaults {
+public:
+ typedef V value_type;
+
+ static const bool is_container = true;
+ static const bool allow_auto_unwrap = true;
+ static const size_t depth = ArrayTraits<V>::depth + 1;
+};
+
+// This handles reference types, such as are given with boost::tie.
+// It also allows for instance "ArrayTraits<T[N]>" to match "ArrayTraits<T (&) [N]>".
+// I think this is okay to do... The alternative is to use remove_reference all over the place.
+template <typename T>
+class ArrayTraits<T&> : public ArrayTraits<T> { };
+
+// FIXME - is this okay?
+// It supports gp.send1d(std::forward_as_tuple(x, std::move(y)));
+#if GNUPLOT_ENABLE_CXX11
+template <typename T>
+class ArrayTraits<T&&> : public ArrayTraits<T> { };
+#endif
+
+// }}}2
+
+// {{{2 STL container support
+
+template <typename TI, typename TV>
+class IteratorRange {
+public:
+ IteratorRange() { }
+ IteratorRange(const TI &_it, const TI &_end) : it(_it), end(_end) { }
+
+ static const bool is_container = ArrayTraits<TV>::is_container;
+ typedef typename boost::mpl::if_c<is_container,
+ Error_InappropriateDeref, TV>::type value_type;
+ typedef typename ArrayTraits<TV>::range_type subiter_type;
+
+ bool is_end() const { return it == end; }
+
+ void inc() { ++it; }
+
+ value_type deref() const {
+ GNUPLOT_STATIC_ASSERT_MSG(sizeof(TV) && !is_container,
+ "deref called on nested container");
+ return *it;
+ }
+
+ subiter_type deref_subiter() const {
+ GNUPLOT_STATIC_ASSERT_MSG(sizeof(TV) && is_container,
+ "deref_iter called on non-nested container");
+ return ArrayTraits<TV>::get_range(*it);
+ }
+
+private:
+ TI it, end;
+};
+
+template <typename T>
+class ArrayTraits<T,
+ typename boost::enable_if<is_like_stl_container<T> >::type
+> : public ArrayTraitsDefaults<typename T::value_type> {
+public:
+ typedef IteratorRange<typename T::const_iterator, typename T::value_type> range_type;
+
+ static range_type get_range(const T &arg) {
+ return range_type(arg.begin(), arg.end());
+ }
+};
+
+// }}}2
+
+// {{{2 C style array support
+
+template <typename T, size_t N>
+class ArrayTraits<T[N]> : public ArrayTraitsDefaults<T> {
+public:
+ typedef IteratorRange<const T*, T> range_type;
+
+ static range_type get_range(const T (&arg)[N]) {
+ return range_type(arg, arg+N);
+ }
+};
+
+// }}}2
+
+// {{{2 std::pair support
+
+template <typename RT, typename RU>
+class PairOfRange {
+ template <typename T, typename U, typename PrintMode>
+ friend void deref_and_print(std::ostream &, const PairOfRange<T, U> &, PrintMode);
+
+public:
+ PairOfRange() { }
+ PairOfRange(const RT &_l, const RU &_r) : l(_l), r(_r) { }
+
+ static const bool is_container = RT::is_container && RU::is_container;
+
+ typedef std::pair<typename RT::value_type, typename RU::value_type> value_type;
+ typedef PairOfRange<typename RT::subiter_type, typename RU::subiter_type> subiter_type;
+
+ bool is_end() const {
+ bool el = l.is_end();
+ bool er = r.is_end();
+ if(el != er) {
+ throw std::length_error("columns were different lengths");
+ }
+ return el;
+ }
+
+ void inc() {
+ l.inc();
+ r.inc();
+ }
+
+ value_type deref() const {
+ return std::make_pair(l.deref(), r.deref());
+ }
+
+ subiter_type deref_subiter() const {
+ return subiter_type(l.deref_subiter(), r.deref_subiter());
+ }
+
+private:
+ RT l;
+ RU r;
+};
+
+template <typename T, typename U>
+class ArrayTraits<std::pair<T, U> > {
+public:
+ typedef PairOfRange<typename ArrayTraits<T>::range_type, typename ArrayTraits<U>::range_type> range_type;
+ typedef std::pair<typename ArrayTraits<T>::value_type, typename ArrayTraits<U>::value_type> value_type;
+ static const bool is_container = ArrayTraits<T>::is_container && ArrayTraits<U>::is_container;
+ // Don't allow colwrap since it's already wrapped.
+ static const bool allow_auto_unwrap = false;
+ // It is allowed for l_depth != r_depth, for example one column could be 'double' and the
+ // other column could be 'vector<double>'.
+ static const size_t l_depth = ArrayTraits<T>::depth;
+ static const size_t r_depth = ArrayTraits<U>::depth;
+ static const size_t depth = (l_depth < r_depth) ? l_depth : r_depth;
+
+ static range_type get_range(const std::pair<T, U> &arg) {
+ return range_type(
+ ArrayTraits<T>::get_range(arg.first),
+ ArrayTraits<U>::get_range(arg.second)
+ );
+ }
+};
+
+// }}}2
+
+// {{{2 boost::tuple support
+
+template <typename T>
+class ArrayTraits<T,
+ typename boost::enable_if<
+ boost::mpl::and_<
+ is_boost_tuple<T>,
+ boost::mpl::not_<is_boost_tuple_nulltype<typename T::tail_type> >
+ >
+ >::type
+> : public ArrayTraits<
+ typename std::pair<
+ typename T::head_type,
+ typename T::tail_type
+ >
+> {
+public:
+ typedef typename T::head_type HT;
+ typedef typename T::tail_type TT;
+
+ typedef ArrayTraits<typename std::pair<HT, TT> > parent;
+
+ static typename parent::range_type get_range(const T &arg) {
+ return typename parent::range_type(
+ ArrayTraits<HT>::get_range(arg.get_head()),
+ ArrayTraits<TT>::get_range(arg.get_tail())
+ );
+ }
+};
+
+template <typename T>
+class ArrayTraits<T,
+ typename boost::enable_if<
+ boost::mpl::and_<
+ is_boost_tuple<T>,
+ is_boost_tuple_nulltype<typename T::tail_type>
+ >
+ >::type
+> : public ArrayTraits<
+ typename T::head_type
+> {
+ typedef typename T::head_type HT;
+
+ typedef ArrayTraits<HT> parent;
+
+public:
+ static typename parent::range_type get_range(const T &arg) {
+ return parent::get_range(arg.get_head());
+ }
+};
+
+// }}}2
+
+// {{{2 std::tuple support
+
+#if GNUPLOT_ENABLE_CXX11
+
+template <typename Tuple, size_t idx>
+struct StdTupUnwinder {
+ typedef std::pair<
+ typename StdTupUnwinder<Tuple, idx-1>::type,
+ typename std::tuple_element<idx, Tuple>::type
+ > type;
+
+ static typename ArrayTraits<type>::range_type get_range(const Tuple &arg) {
+ return typename ArrayTraits<type>::range_type(
+ StdTupUnwinder<Tuple, idx-1>::get_range(arg),
+ ArrayTraits<typename std::tuple_element<idx, Tuple>::type>::get_range(std::get<idx>(arg))
+ );
+ }
+};
+
+template <typename Tuple>
+struct StdTupUnwinder<Tuple, 0> {
+ typedef typename std::tuple_element<0, Tuple>::type type;
+
+ static typename ArrayTraits<type>::range_type get_range(const Tuple &arg) {
+ return ArrayTraits<type>::get_range(std::get<0>(arg));
+ }
+};
+
+template <typename... Args>
+class ArrayTraits<std::tuple<Args...> > :
+ public ArrayTraits<typename StdTupUnwinder<std::tuple<Args...>, sizeof...(Args)-1>::type>
+{
+ typedef std::tuple<Args...> Tuple;
+ typedef ArrayTraits<typename StdTupUnwinder<Tuple, sizeof...(Args)-1>::type> parent;
+
+public:
+ static typename parent::range_type get_range(const Tuple &arg) {
+ return StdTupUnwinder<std::tuple<Args...>, sizeof...(Args)-1>::get_range(arg);
+ }
+};
+
+#endif // GNUPLOT_ENABLE_CXX11
+
+// }}}2
+
+// {{{2 Support column unwrap of container (VecOfRange)
+//
+// VecOfRange (created via `get_columns_range()`) treats the outermost level of a nested
+// container as if it were a tuple. Since tuples are sent to gnuplot as columns, this has the
+// effect of addressing a multidimensional array in column major order.
+
+template <typename RT>
+class VecOfRange {
+ template <typename T, typename PrintMode>
+ friend void deref_and_print(std::ostream &, const VecOfRange<T> &, PrintMode);
+
+public:
+ VecOfRange() { }
+ explicit VecOfRange(const std::vector<RT> &_rvec) : rvec(_rvec) { }
+
+ static const bool is_container = RT::is_container;
+ // Don't allow colwrap since it's already wrapped.
+ static const bool allow_auto_unwrap = false;
+
+ typedef std::vector<typename RT::value_type> value_type;
+ typedef VecOfRange<typename RT::subiter_type> subiter_type;
+
+ bool is_end() const {
+ if(rvec.empty()) return true;
+ bool ret = rvec[0].is_end();
+ for(size_t i=1; i<rvec.size(); i++) {
+ if(ret != rvec[i].is_end()) {
+ throw std::length_error("columns were different lengths");
+ }
+ }
+ return ret;
+ }
+
+ void inc() {
+ for(size_t i=0; i<rvec.size(); i++) {
+ rvec[i].inc();
+ }
+ }
+
+ value_type deref() const {
+ value_type ret(rvec.size());
+ for(size_t i=0; i<rvec.size(); i++) {
+ ret[i] = rvec[i].deref();
+ }
+ return ret;
+ }
+
+ subiter_type deref_subiter() const {
+ std::vector<typename RT::subiter_type> subvec(rvec.size());
+ for(size_t i=0; i<rvec.size(); i++) {
+ subvec[i] = rvec[i].deref_subiter();
+ }
+ return subiter_type(subvec);
+ }
+
+private:
+ std::vector<RT> rvec;
+};
+
+template <typename T>
+VecOfRange<typename ArrayTraits<T>::range_type::subiter_type>
+get_columns_range(const T &arg) {
+ typedef typename ArrayTraits<T>::range_type::subiter_type U;
+ std::vector<U> rvec;
+ typename ArrayTraits<T>::range_type outer = ArrayTraits<T>::get_range(arg);
+ while(!outer.is_end()) {
+ rvec.push_back(outer.deref_subiter());
+ outer.inc();
+ }
+ VecOfRange<U> ret(rvec);
+ return ret;
+}
+
+// }}}2
+
+// }}}1
+
+// {{{1 Array printing functions
+//
+// This section coordinates the sending of data to gnuplot. The ArrayTraits mechanism tells us
+// about nested containers and provides iterators over them. Here we make use of this,
+// deciding what dimensions should be treated as rows, columns, or blocks, telling gnuplot the
+// size of the array if needed, and so on.
+
+// If this is set, then text-mode data will be sent in a format that is not compatible with
+// gnuplot, but which helps the programmer tell what the library is thinking. Basically it
+// puts brackets around groups of items and puts a message delineating blocks of data.
+static bool debug_array_print = 0;
+void set_debug_array_print(bool v) { debug_array_print = v; }
+
+// {{{2 Tags (like enums for metaprogramming)
+
+// These tags define what our goal is, what sort of thing should ultimately be sent to the
+// ostream. These tags are passed to the PrintMode template argument of the functions in this
+// section.
+//
+// ModeText - Sends the data in an array in text format
+// ModeBinary - Sends the data in an array in binary format
+// ModeBinfmt - Sends the gnuplot format code for binary data (e.g. "%double%double")
+// ModeSize - Sends the size of an array. Needed when sending binary data.
+struct ModeText { static const bool is_text = 1; static const bool is_binfmt = 0; static const bool is_size = 0; };
+struct ModeBinary { static const bool is_text = 0; static const bool is_binfmt = 0; static const bool is_size = 0; };
+struct ModeBinfmt { static const bool is_text = 0; static const bool is_binfmt = 1; static const bool is_size = 0; };
+struct ModeSize { static const bool is_text = 0; static const bool is_binfmt = 0; static const bool is_size = 1; };
+
+// Whether to treat the outermost level of a nested container as columns (column major mode).
+struct ColUnwrapNo { };
+struct ColUnwrapYes { };
+
+// The user must give a hint to describe how nested containers are to be interpreted. This is
+// done by calling e.g. `send1d_colmajor()` or `send2d()`. This hint is then described by the
+// following tags. This is passed to the OrganizationMode template argument.
+struct Mode1D { static std::string class_name() { return "Mode1D" ; } };
+struct Mode2D { static std::string class_name() { return "Mode2D" ; } };
+struct Mode1DUnwrap { static std::string class_name() { return "Mode1DUnwrap"; } };
+struct Mode2DUnwrap { static std::string class_name() { return "Mode2DUnwrap"; } };
+// Support for the legacy behavior that guesses which of the above four modes should be used.
+struct ModeAuto { static std::string class_name() { return "ModeAuto" ; } };
+
+// }}}2
+
+// {{{2 ModeAutoDecoder
+//
+// ModeAuto guesses which of Mode1D, Mode2D, Mode1DUnwrap, or Mode2DUnwrap should be used.
+// This is provided for reverse compatibility; it is better to specify explicitly which mode to
+// use. Since this is only for reverse compatibility, and shouldn't be used, I'm not going to
+// spell out what the rules are. See below for details.
+
+template <typename T, typename Enable=void>
+struct ModeAutoDecoder { };
+
+template <typename T>
+struct ModeAutoDecoder<T,
+ typename boost::enable_if_c<
+ (ArrayTraits<T>::depth == 1)
+ >::type>
+{
+ typedef Mode1D mode;
+};
+
+template <typename T>
+struct ModeAutoDecoder<T,
+ typename boost::enable_if_c<
+ (ArrayTraits<T>::depth == 2) &&
+ !ArrayTraits<T>::allow_auto_unwrap
+ >::type>
+{
+ typedef Mode2D mode;
+};
+
+template <typename T>
+struct ModeAutoDecoder<T,
+ typename boost::enable_if_c<
+ (ArrayTraits<T>::depth == 2) &&
+ ArrayTraits<T>::allow_auto_unwrap
+ >::type>
+{
+ typedef Mode1DUnwrap mode;
+};
+
+template <typename T>
+struct ModeAutoDecoder<T,
+ typename boost::enable_if_c<
+ (ArrayTraits<T>::depth > 2) &&
+ ArrayTraits<T>::allow_auto_unwrap
+ >::type>
+{
+ typedef Mode2DUnwrap mode;
+};
+
+template <typename T>
+struct ModeAutoDecoder<T,
+ typename boost::enable_if_c<
+ (ArrayTraits<T>::depth > 2) &&
+ !ArrayTraits<T>::allow_auto_unwrap
+ >::type>
+{
+ typedef Mode2D mode;
+};
+
+// }}}2
+
+// The data is processed using several levels of functions that call each other in sequence,
+// each defined in a subsection of code below. Because C++ wants you to declare a function
+// before using it, we begin with the innermost function. So in order to see the sequence in
+// which these are called, you should read the following subsections in reverse order. Nested
+// arrays are formated into blocks (for 2D data) and lines (for 1D or 2D data), then further
+// nesting levels are formatted into columns. Also tag dispatching is used in order to define
+// various sorts of behavior. Each of these tasks is handled by one of the following
+// subsections.
+
+// {{{2 send_scalar()
+//
+// Send a scalar in one of three possible ways: via TextSender, BinarySender, or BinfmtSender,
+// depending on which PrintMode tag is passed to the function.
+
+template <typename T>
+void send_scalar(std::ostream &stream, const T &arg, ModeText) {
+ TextSender<T>::send(stream, arg);
+}
+
+template <typename T>
+void send_scalar(std::ostream &stream, const T &arg, ModeBinary) {
+ BinarySender<T>::send(stream, arg);
+}
+
+template <typename T>
+void send_scalar(std::ostream &stream, const T &, ModeBinfmt) {
+ BinfmtSender<T>::send(stream);
+}
+
+// }}}2
+
+// {{{2 deref_and_print()
+//
+// Dereferences and prints the given range (iterator). At this point we are done with treating
+// containers as blocks (for 2D data) and lines (for 1D or 2D data). Any further levels of
+// nested containers will at this point be treated as columns.
+
+// If arg is not a container, then print it via send_scalar().
+template <typename T, typename PrintMode>
+typename boost::disable_if_c<T::is_container>::type
+deref_and_print(std::ostream &stream, const T &arg, PrintMode) {
+ const typename T::value_type &v = arg.deref();
+ send_scalar(stream, v, PrintMode());
+}
+
+// If arg is a container (but not a PairOfRange or VecOfRange, which are handled below) then
+// treat the contents as columns, iterating over the contents recursively. If outputting in
+// text mode, put a space between columns.
+template <typename T, typename PrintMode>
+typename boost::enable_if_c<T::is_container>::type
+deref_and_print(std::ostream &stream, const T &arg, PrintMode) {
+ if(debug_array_print && PrintMode::is_text) stream << "{";
+ typename T::subiter_type subrange = arg.deref_subiter();
+ bool first = true;
+ while(!subrange.is_end()) {
+ if(!first && PrintMode::is_text) stream << " ";
+ first = false;
+ deref_and_print(stream, subrange, PrintMode());
+ subrange.inc();
+ }
+ if(debug_array_print && PrintMode::is_text) stream << "}";
+}
+
+// PairOfRange is treated as columns. In text mode, put a space between columns.
+template <typename T, typename U, typename PrintMode>
+void deref_and_print(std::ostream &stream, const PairOfRange<T, U> &arg, PrintMode) {
+ deref_and_print(stream, arg.l, PrintMode());
+ if(PrintMode::is_text) stream << " ";
+ deref_and_print(stream, arg.r, PrintMode());
+}
+
+// VecOfRange is treated as columns. In text mode, put a space between columns.
+template <typename T, typename PrintMode>
+void deref_and_print(std::ostream &stream, const VecOfRange<T> &arg, PrintMode) {
+ for(size_t i=0; i<arg.rvec.size(); i++) {
+ if(i && PrintMode::is_text) stream << " ";
+ deref_and_print(stream, arg.rvec[i], PrintMode());
+ }
+}
+
+// }}}2
+
+// {{{2 print_block()
+//
+// Here we format nested containers into blocks (for 2D data) and lines. Actually, block and
+// line formatting is only truely needed for text mode output, but for uniformity this function
+// is also invoked in binary mode (the PrintMode tag determines the output mode). If the goal
+// is to just print the array size or the binary format string, then the loops exit after the
+// first iteration.
+//
+// The Depth argument tells how deep to recurse. It will be either `2` for 2D data, formatted
+// into blocks and lines, with empty lines between blocks, or `1` for 1D data formatted into
+// lines but not blocks. Gnuplot only supports 1D and 2D data, but if it were to support 3D in
+// the future (e.g. volume rendering), all that would be needed would be some trivial changes
+// in this section. After Depth number of nested containers have been recursed into, control
+// is passed to deref_and_print(), which treats any further nested containers as columns.
+
+// Depth==1 and we are not asked to print the size of the array. Send each element of the
+// range to deref_and_print() for further processing into columns.
+template <size_t Depth, typename T, typename PrintMode>
+typename boost::enable_if_c<(Depth==1) && !PrintMode::is_size>::type
+print_block(std::ostream &stream, T &arg, PrintMode) {
+ for(; !arg.is_end(); arg.inc()) {
+ //print_entry(arg.deref());
+ deref_and_print(stream, arg, PrintMode());
+ // If asked to print the binary format string, only the first element needs to be
+ // looked at.
+ if(PrintMode::is_binfmt) break;
+ if(PrintMode::is_text) stream << std::endl;
+ }
+}
+
+// Depth>1 and we are not asked to print the size of the array. Loop over the range and
+// recurse into print_block() with Depth -> Depth-1.
+template <size_t Depth, typename T, typename PrintMode>
+typename boost::enable_if_c<(Depth>1) && !PrintMode::is_size>::type
+print_block(std::ostream &stream, T &arg, PrintMode) {
+ bool first = true;
+ for(; !arg.is_end(); arg.inc()) {
+ if(first) {
+ first = false;
+ } else {
+ if(PrintMode::is_text) stream << std::endl;
+ }
+ if(debug_array_print && PrintMode::is_text) stream << "<block>" << std::endl;
+ typename T::subiter_type sub = arg.deref_subiter();
+ print_block<Depth-1>(stream, sub, PrintMode());
+ // If asked to print the binary format string, only the first element needs to be
+ // looked at.
+ if(PrintMode::is_binfmt) break;
+ }
+}
+
+// Determine how many elements are in the given range. Used in the functions below.
+template <typename T>
+size_t get_range_size(const T &arg) {
+ // FIXME - not the fastest way. Implement a size() method for range.
+ size_t ret = 0;
+ for(T i=arg; !i.is_end(); i.inc()) ++ret;
+ return ret;
+}
+
+// Depth==1 and we are asked to print the size of the array.
+template <size_t Depth, typename T, typename PrintMode>
+typename boost::enable_if_c<(Depth==1) && PrintMode::is_size>::type
+print_block(std::ostream &stream, T &arg, PrintMode) {
+ stream << get_range_size(arg);
+}
+
+// Depth>1 and we are asked to print the size of the array.
+template <size_t Depth, typename T, typename PrintMode>
+typename boost::enable_if_c<(Depth>1) && PrintMode::is_size>::type
+print_block(std::ostream &stream, T &arg, PrintMode) {
+ // It seems that size for two dimensional arrays needs the fastest varying index first,
+ // contrary to intuition. The gnuplot documentation is not too clear on this point.
+ typename T::subiter_type sub = arg.deref_subiter();
+ print_block<Depth-1>(stream, sub, PrintMode());
+ stream << "," << get_range_size(arg);
+}
+
+// }}}2
+
+// {{{2 handle_colunwrap_tag()
+//
+// If passed the ColUnwrapYes then treat the outermost nested container as columns by calling
+// get_columns_range(). Otherwise just call get_range(). The range iterator is then passed to
+// print_block() for further processing.
+
+template <size_t Depth, typename T, typename PrintMode>
+void handle_colunwrap_tag(std::ostream &stream, const T &arg, ColUnwrapNo, PrintMode) {
+ GNUPLOT_STATIC_ASSERT_MSG(ArrayTraits<T>::depth >= Depth, "container not deep enough");
+ typename ArrayTraits<T>::range_type range = ArrayTraits<T>::get_range(arg);
+ print_block<Depth>(stream, range, PrintMode());
+}
+
+template <size_t Depth, typename T, typename PrintMode>
+void handle_colunwrap_tag(std::ostream &stream, const T &arg, ColUnwrapYes, PrintMode) {
+ GNUPLOT_STATIC_ASSERT_MSG(ArrayTraits<T>::depth >= Depth+1, "container not deep enough");
+ VecOfRange<typename ArrayTraits<T>::range_type::subiter_type> cols = get_columns_range(arg);
+ print_block<Depth>(stream, cols, PrintMode());
+}
+
+// }}}2
+
+// {{{2 handle_organization_tag()
+//
+// Parse the OrganizationMode tag then forward to handle_colunwrap_tag() for further
+// processing. If passed the Mode1D or Mode2D tags, then set Depth=1 or Depth=2. If passed
+// Mode{1,2}DUnwrap then use the ColUnwrapYes tag. If passed ModeAuto (which is for legacy
+// support) then use ModeAutoDecoder to guess which of Mode1D, Mode2D, etc. should be used.
+
+template <typename T, typename PrintMode>
+void handle_organization_tag(std::ostream &stream, const T &arg, Mode1D, PrintMode) {
+ handle_colunwrap_tag<1>(stream, arg, ColUnwrapNo(), PrintMode());
+}
+
+template <typename T, typename PrintMode>
+void handle_organization_tag(std::ostream &stream, const T &arg, Mode2D, PrintMode) {
+ handle_colunwrap_tag<2>(stream, arg, ColUnwrapNo(), PrintMode());
+}
+
+template <typename T, typename PrintMode>
+void handle_organization_tag(std::ostream &stream, const T &arg, Mode1DUnwrap, PrintMode) {
+ handle_colunwrap_tag<1>(stream, arg, ColUnwrapYes(), PrintMode());
+}
+
+template <typename T, typename PrintMode>
+void handle_organization_tag(std::ostream &stream, const T &arg, Mode2DUnwrap, PrintMode) {
+ handle_colunwrap_tag<2>(stream, arg, ColUnwrapYes(), PrintMode());
+}
+
+template <typename T, typename PrintMode>
+void handle_organization_tag(std::ostream &stream, const T &arg, ModeAuto, PrintMode) {
+ handle_organization_tag(stream, arg, typename ModeAutoDecoder<T>::mode(), PrintMode());
+}
+
+// }}}2
+
+// The entry point for the processing defined in this section. It just forwards immediately to
+// handle_organization_tag(). This function is only here to give a sane name to the entry
+// point.
+//
+// The allowed values for the OrganizationMode and PrintMode tags are defined in the beginning
+// of this section.
+template <typename T, typename OrganizationMode, typename PrintMode>
+void top_level_array_sender(std::ostream &stream, const T &arg, OrganizationMode, PrintMode) {
+ handle_organization_tag(stream, arg, OrganizationMode(), PrintMode());
+}
+
+// }}}1
+
+// {{{1 FileHandleWrapper
+
+// This holds the file handle that gnuplot commands will be sent to. The purpose of this
+// wrapper is twofold:
+// 1. It allows storing the FILE* before it gets passed to the boost::iostreams::stream
+// constructor (which is a base class of the main Gnuplot class). This is accomplished
+// via multiple inheritance as described at http://stackoverflow.com/a/3821756/1048959
+// 2. It remembers whether the handle needs to be closed via fclose or pclose.
+struct FileHandleWrapper {
+ FileHandleWrapper(std::FILE *_fh, bool _should_use_pclose) :
+ wrapped_fh(_fh), should_use_pclose(_should_use_pclose) { }
+
+ void fh_close() {
+ if(should_use_pclose) {
+ if(GNUPLOT_PCLOSE(wrapped_fh)) {
+ std::cerr << "pclose returned error" << std::endl;
+ }
+ } else {
+ if(fclose(wrapped_fh)) {
+ std::cerr << "fclose returned error" << std::endl;
+ }
+ }
+ }
+
+ int fh_fileno() {
+ return GNUPLOT_FILENO(wrapped_fh);
+ }
+
+ std::FILE *wrapped_fh;
+ bool should_use_pclose;
+};
+
+// }}}1
+
+// {{{1 Main class
+
+class Gnuplot :
+ // Some setup needs to be done before obtaining the file descriptor that gets passed to
+ // boost::iostreams::stream. This is accomplished by using a multiple inheritance trick,
+ // as described at http://stackoverflow.com/a/3821756/1048959
+ private FileHandleWrapper,
+ public boost::iostreams::stream<boost::iostreams::file_descriptor_sink>
+{
+private:
+ static std::string get_default_cmd() {
+ GNUPLOT_MSVC_WARNING_4996_PUSH
+ char *from_env = std::getenv("GNUPLOT_IOSTREAM_CMD");
+ GNUPLOT_MSVC_WARNING_4996_POP
+ if(from_env && from_env[0]) {
+ return from_env;
+ } else {
+ return GNUPLOT_DEFAULT_COMMAND;
+ }
+ }
+
+ static FileHandleWrapper open_cmdline(const std::string &in) {
+ std::string cmd = in.empty() ? get_default_cmd() : in;
+ assert(!cmd.empty());
+ if(cmd[0] == '>') {
+ std::string fn = cmd.substr(1);
+ GNUPLOT_MSVC_WARNING_4996_PUSH
+ FILE *fh = std::fopen(fn.c_str(), "w");
+ GNUPLOT_MSVC_WARNING_4996_POP
+ if(!fh) throw(std::ios_base::failure("cannot open file "+fn));
+ return FileHandleWrapper(fh, false);
+ } else {
+ FILE *fh = GNUPLOT_POPEN(cmd.c_str(), "w");
+ if(!fh) throw(std::ios_base::failure("cannot open pipe "+cmd));
+ return FileHandleWrapper(fh, true);
+ }
+ }
+
+public:
+ explicit Gnuplot(const std::string &_cmd="") :
+ FileHandleWrapper(open_cmdline(_cmd)),
+ boost::iostreams::stream<boost::iostreams::file_descriptor_sink>(
+ fh_fileno(),
+#if BOOST_VERSION >= 104400
+ boost::iostreams::never_close_handle
+#else
+ false
+#endif
+ ),
+ feedback(NULL),
+ tmp_files(),
+ debug_messages(false)
+ {
+ *this << std::scientific << std::setprecision(18); // refer <iomanip>
+ }
+
+ explicit Gnuplot(FILE *_fh) :
+ FileHandleWrapper(_fh, 0),
+ boost::iostreams::stream<boost::iostreams::file_descriptor_sink>(
+ fh_fileno(),
+#if BOOST_VERSION >= 104400
+ boost::iostreams::never_close_handle
+#else
+ false
+#endif
+ ),
+ feedback(NULL),
+ tmp_files(),
+ debug_messages(false)
+ {
+ *this << std::scientific << std::setprecision(18); // refer <iomanip>
+ }
+
+private:
+ // noncopyable
+ Gnuplot(const Gnuplot &);
+ const Gnuplot& operator=(const Gnuplot &);
+
+public:
+ ~Gnuplot() {
+ if(debug_messages) {
+ std::cerr << "ending gnuplot session" << std::endl;
+ }
+
+ // FIXME - boost's close method calls close() on the file descriptor, but we need to
+ // use sometimes use pclose instead. For now, just skip calling boost's close and use
+ // flush just in case.
+ do_flush();
+ // Wish boost had a pclose method...
+ //close();
+
+ fh_close();
+
+ delete feedback;
+ }
+
+ void clearTmpfiles() {
+ // destructors will cause deletion
+ tmp_files.clear();
+ }
+
+private:
+ void do_flush() {
+ *this << std::flush;
+ fflush(wrapped_fh);
+ }
+
+ std::string make_tmpfile() {
+#ifdef GNUPLOT_USE_TMPFILE
+ boost::shared_ptr<GnuplotTmpfile> tmp_file(new GnuplotTmpfile());
+ // The file will be removed once the pointer is removed from the
+ // tmp_files container.
+ tmp_files.push_back(tmp_file);
+ return tmp_file->file.string();
+#else
+ throw(std::logic_error("no filename given and temporary files not enabled"));
+#endif // GNUPLOT_USE_TMPFILE
+ }
+
+public:
+// {{{2 Generic sender routines.
+//
+// These are declared public, but are undocumented. It is recommended to use the functions in
+// the next section, which serve as adapters that pass specific values for the OrganizationMode
+// tag.
+
+ template <typename T, typename OrganizationMode>
+ Gnuplot &send(const T &arg, OrganizationMode) {
+ top_level_array_sender(*this, arg, OrganizationMode(), ModeText());
+ *this << "e" << std::endl; // gnuplot's "end of array" token
+ do_flush(); // probably not really needed, but doesn't hurt
+ return *this;
+ }
+
+ template <typename T, typename OrganizationMode>
+ Gnuplot &sendBinary(const T &arg, OrganizationMode) {
+ top_level_array_sender(*this, arg, OrganizationMode(), ModeBinary());
+ do_flush(); // probably not really needed, but doesn't hurt
+ return *this;
+ }
+
+ template <typename T, typename OrganizationMode>
+ std::string binfmt(const T &arg, const std::string &arr_or_rec, OrganizationMode) {
+ std::ostringstream tmp;
+ tmp << " format='";
+ top_level_array_sender(tmp, arg, OrganizationMode(), ModeBinfmt());
+ assert((arr_or_rec == "array") || (arr_or_rec == "record"));
+ tmp << "' " << arr_or_rec << "=(";
+ top_level_array_sender(tmp, arg, OrganizationMode(), ModeSize());
+ tmp << ")";
+ tmp << " ";
+ return tmp.str();
+ }
+
+ // NOTE: empty filename makes temporary file
+ template <typename T, typename OrganizationMode>
+ std::string file(const T &arg, std::string filename, OrganizationMode) {
+ if(filename.empty()) filename = make_tmpfile();
+ std::fstream tmp_stream(filename.c_str(), std::fstream::out);
+ top_level_array_sender(tmp_stream, arg, OrganizationMode(), ModeText());
+ tmp_stream.close();
+
+ std::ostringstream cmdline;
+ // FIXME - hopefully filename doesn't contain quotes or such...
+ cmdline << " '" << filename << "' ";
+ return cmdline.str();
+ }
+
+ // NOTE: empty filename makes temporary file
+ template <typename T, typename OrganizationMode>
+ std::string binaryFile(const T &arg, std::string filename, const std::string &arr_or_rec, OrganizationMode) {
+ if(filename.empty()) filename = make_tmpfile();
+ std::fstream tmp_stream(filename.c_str(), std::fstream::out | std::fstream::binary);
+ top_level_array_sender(tmp_stream, arg, OrganizationMode(), ModeBinary());
+ tmp_stream.close();
+
+ std::ostringstream cmdline;
+ // FIXME - hopefully filename doesn't contain quotes or such...
+ cmdline << " '" << filename << "' binary" << binfmt(arg, arr_or_rec, OrganizationMode());
+ return cmdline.str();
+ }
+
+// }}}2
+
+// {{{2 Deprecated data sending interface that guesses an appropriate OrganizationMode. This is here
+// for reverse compatibility. Don't use it. A warning will be printed if
+// GNUPLOT_DEPRECATE_WARN is defined.
+
+ template <typename T> Gnuplot GNUPLOT_DEPRECATE("use send1d or send2d")
+ &send(const T &arg) { return send(arg, ModeAuto()); }
+
+ template <typename T> std::string GNUPLOT_DEPRECATE("use binfmt1d or binfmt2d")
+ binfmt(const T &arg, const std::string &arr_or_rec="array")
+ { return binfmt(arg, arr_or_rec, ModeAuto()); }
+
+ template <typename T> Gnuplot GNUPLOT_DEPRECATE("use sendBinary1d or sendBinary2d")
+ &sendBinary(const T &arg) { return sendBinary(arg, ModeAuto()); }
+
+ template <typename T> std::string GNUPLOT_DEPRECATE("use file1d or file2d")
+ file(const T &arg, const std::string &filename="")
+ { return file(arg, filename, ModeAuto()); }
+
+ template <typename T> std::string GNUPLOT_DEPRECATE("use binArr1d or binArr2d")
+ binaryFile(const T &arg, const std::string &filename="", const std::string &arr_or_rec="array")
+ { return binaryFile(arg, filename, arr_or_rec, ModeAuto()); }
+
+// }}}2
+
+// {{{2 Public (documented) data sending interface.
+//
+// It seems odd to define 16 different functions, but I think this ends up being the most
+// convenient in terms of usage by the end user.
+
+ template <typename T> Gnuplot &send1d (const T &arg) { return send(arg, Mode1D ()); }
+ template <typename T> Gnuplot &send2d (const T &arg) { return send(arg, Mode2D ()); }
+ template <typename T> Gnuplot &send1d_colmajor(const T &arg) { return send(arg, Mode1DUnwrap()); }
+ template <typename T> Gnuplot &send2d_colmajor(const T &arg) { return send(arg, Mode2DUnwrap()); }
+
+ template <typename T> Gnuplot &sendBinary1d (const T &arg) { return sendBinary(arg, Mode1D ()); }
+ template <typename T> Gnuplot &sendBinary2d (const T &arg) { return sendBinary(arg, Mode2D ()); }
+ template <typename T> Gnuplot &sendBinary1d_colmajor(const T &arg) { return sendBinary(arg, Mode1DUnwrap()); }
+ template <typename T> Gnuplot &sendBinary2d_colmajor(const T &arg) { return sendBinary(arg, Mode2DUnwrap()); }
+
+ template <typename T> std::string file1d (const T &arg, const std::string &filename="") { return file(arg, filename, Mode1D ()); }
+ template <typename T> std::string file2d (const T &arg, const std::string &filename="") { return file(arg, filename, Mode2D ()); }
+ template <typename T> std::string file1d_colmajor(const T &arg, const std::string &filename="") { return file(arg, filename, Mode1DUnwrap()); }
+ template <typename T> std::string file2d_colmajor(const T &arg, const std::string &filename="") { return file(arg, filename, Mode2DUnwrap()); }
+
+ template <typename T> std::string binFmt1d (const T &arg, const std::string &arr_or_rec) { return binfmt(arg, arr_or_rec, Mode1D ()); }
+ template <typename T> std::string binFmt2d (const T &arg, const std::string &arr_or_rec) { return binfmt(arg, arr_or_rec, Mode2D ()); }
+ template <typename T> std::string binFmt1d_colmajor(const T &arg, const std::string &arr_or_rec) { return binfmt(arg, arr_or_rec, Mode1DUnwrap()); }
+ template <typename T> std::string binFmt2d_colmajor(const T &arg, const std::string &arr_or_rec) { return binfmt(arg, arr_or_rec, Mode2DUnwrap()); }
+
+ template <typename T> std::string binFile1d (const T &arg, const std::string &arr_or_rec, const std::string &filename="") { return binaryFile(arg, filename, arr_or_rec, Mode1D ()); }
+ template <typename T> std::string binFile2d (const T &arg, const std::string &arr_or_rec, const std::string &filename="") { return binaryFile(arg, filename, arr_or_rec, Mode2D ()); }
+ template <typename T> std::string binFile1d_colmajor(const T &arg, const std::string &arr_or_rec, const std::string &filename="") { return binaryFile(arg, filename, arr_or_rec, Mode1DUnwrap()); }
+ template <typename T> std::string binFile2d_colmajor(const T &arg, const std::string &arr_or_rec, const std::string &filename="") { return binaryFile(arg, filename, arr_or_rec, Mode2DUnwrap()); }
+
+// }}}2
+
+#ifdef GNUPLOT_ENABLE_FEEDBACK
+public:
+ // Input variables are set to the mouse position and button. If the gnuplot
+ // window is closed, button -1 is returned. The msg parameter is the prompt
+ // that is printed to the console.
+ void getMouse(
+ double &mx, double &my, int &mb,
+ std::string msg="Click Mouse!"
+ ) {
+ allocFeedback();
+
+ *this << "set mouse" << std::endl;
+ *this << "pause mouse \"" << msg << "\\n\"" << std::endl;
+ *this << "if (exists(\"MOUSE_X\")) print MOUSE_X, MOUSE_Y, MOUSE_BUTTON; else print 0, 0, -1;" << std::endl;
+ if(debug_messages) {
+ std::cerr << "begin scanf" << std::endl;
+ }
+ if(3 != fscanf(feedback->handle(), "%50lf %50lf %50d", &mx, &my, &mb)) {
+ throw std::runtime_error("could not parse reply");
+ }
+ if(debug_messages) {
+ std::cerr << "end scanf" << std::endl;
+ }
+ }
+
+private:
+ void allocFeedback() {
+ if(!feedback) {
+#ifdef GNUPLOT_ENABLE_PTY
+ feedback = new GnuplotFeedbackPty(debug_messages);
+//#elif defined GNUPLOT_USE_TMPFILE
+//// Currently this doesn't work since fscanf doesn't block (need something like "tail -f")
+// feedback = new GnuplotFeedbackTmpfile(debug_messages);
+#else
+ // This shouldn't happen because we are in an `#ifdef GNUPLOT_ENABLE_FEEDBACK`
+ // block which should only be activated if GNUPLOT_ENABLE_PTY is defined.
+ GNUPLOT_STATIC_ASSERT_MSG((sizeof(T) == 0), "No feedback mechanism defined.");
+#endif
+ *this << "set print \"" << feedback->filename() << "\"" << std::endl;
+ }
+ }
+#endif // GNUPLOT_ENABLE_FEEDBACK
+
+private:
+ GnuplotFeedback *feedback;
+#ifdef GNUPLOT_USE_TMPFILE
+ std::vector<boost::shared_ptr<GnuplotTmpfile> > tmp_files;
+#else
+ // just a placeholder
+ std::vector<int> tmp_files;
+#endif // GNUPLOT_USE_TMPFILE
+
+public:
+ bool debug_messages;
+};
+
+// }}}1
+
+} // namespace gnuplotio
+
+// The first version of this library didn't use namespaces, and now this must be here forever
+// for reverse compatibility.
+using gnuplotio::Gnuplot;
+
+#endif // GNUPLOT_IOSTREAM_H
+
+// {{{1 Support for 3rd party array libraries
+
+// {{{2 Blitz support
+
+// This is outside of the main header guard so that it will be compiled when people do
+// something like this:
+// #include "gnuplot-iostream.h"
+// #include <blitz/array.h>
+// #include "gnuplot-iostream.h"
+// Note that it has its own header guard to avoid double inclusion.
+
+#ifdef BZ_BLITZ_H
+#ifndef GNUPLOT_BLITZ_SUPPORT_LOADED
+#define GNUPLOT_BLITZ_SUPPORT_LOADED
+namespace gnuplotio {
+
+template <typename T, int N>
+struct BinfmtSender<blitz::TinyVector<T, N> > {
+ static void send(std::ostream &stream) {
+ for(int i=0; i<N; i++) {
+ BinfmtSender<T>::send(stream);
+ }
+ }
+};
+
+template <typename T, int N>
+struct TextSender<blitz::TinyVector<T, N> > {
+ static void send(std::ostream &stream, const blitz::TinyVector<T, N> &v) {
+ for(int i=0; i<N; i++) {
+ if(i) stream << " ";
+ TextSender<T>::send(stream, v[i]);
+ }
+ }
+};
+
+template <typename T, int N>
+struct BinarySender<blitz::TinyVector<T, N> > {
+ static void send(std::ostream &stream, const blitz::TinyVector<T, N> &v) {
+ for(int i=0; i<N; i++) {
+ BinarySender<T>::send(stream, v[i]);
+ }
+ }
+};
+
+class Error_WasBlitzPartialSlice { };
+
+template <typename T, int ArrayDim, int SliceDim>
+class BlitzIterator {
+public:
+ BlitzIterator() : p(NULL) { }
+ BlitzIterator(
+ const blitz::Array<T, ArrayDim> *_p,
+ const blitz::TinyVector<int, ArrayDim> _idx
+ ) : p(_p), idx(_idx) { }
+
+ typedef Error_WasBlitzPartialSlice value_type;
+ typedef BlitzIterator<T, ArrayDim, SliceDim-1> subiter_type;
+ static const bool is_container = true;
+
+ // FIXME - it would be nice to also handle one-based arrays
+ bool is_end() const {
+ return idx[ArrayDim-SliceDim] == p->shape()[ArrayDim-SliceDim];
+ }
+
+ void inc() {
+ ++idx[ArrayDim-SliceDim];
+ }
+
+ value_type deref() const {
+ GNUPLOT_STATIC_ASSERT_MSG((sizeof(T) == 0), "cannot deref a blitz slice");
+ throw std::logic_error("static assert should have been triggered by this point");
+ }
+
+ subiter_type deref_subiter() const {
+ return BlitzIterator<T, ArrayDim, SliceDim-1>(p, idx);
+ }
+
+private:
+ const blitz::Array<T, ArrayDim> *p;
+ blitz::TinyVector<int, ArrayDim> idx;
+};
+
+template <typename T, int ArrayDim>
+class BlitzIterator<T, ArrayDim, 1> {
+public:
+ BlitzIterator() : p(NULL) { }
+ BlitzIterator(
+ const blitz::Array<T, ArrayDim> *_p,
+ const blitz::TinyVector<int, ArrayDim> _idx
+ ) : p(_p), idx(_idx) { }
+
+ typedef T value_type;
+ typedef Error_WasNotContainer subiter_type;
+ static const bool is_container = false;
+
+ // FIXME - it would be nice to also handle one-based arrays
+ bool is_end() const {
+ return idx[ArrayDim-1] == p->shape()[ArrayDim-1];
+ }
+
+ void inc() {
+ ++idx[ArrayDim-1];
+ }
+
+ value_type deref() const {
+ return (*p)(idx);
+ }
+
+ subiter_type deref_subiter() const {
+ GNUPLOT_STATIC_ASSERT_MSG((sizeof(T) == 0), "argument was not a container");
+ throw std::logic_error("static assert should have been triggered by this point");
+ }
+
+private:
+ const blitz::Array<T, ArrayDim> *p;
+ blitz::TinyVector<int, ArrayDim> idx;
+};
+
+template <typename T, int ArrayDim>
+class ArrayTraits<blitz::Array<T, ArrayDim> > : public ArrayTraitsDefaults<T> {
+public:
+ static const bool allow_auto_unwrap = false;
+ static const size_t depth = ArrayTraits<T>::depth + ArrayDim;
+
+ typedef BlitzIterator<T, ArrayDim, ArrayDim> range_type;
+
+ static range_type get_range(const blitz::Array<T, ArrayDim> &arg) {
+ blitz::TinyVector<int, ArrayDim> start_idx;
+ start_idx = 0;
+ return range_type(&arg, start_idx);
+ }
+};
+
+} // namespace gnuplotio
+#endif // GNUPLOT_BLITZ_SUPPORT_LOADED
+#endif // BZ_BLITZ_H
+
+// }}}2
+
+// {{{2 Armadillo support
+
+// This is outside of the main header guard so that it will be compiled when people do
+// something like this:
+// #include "gnuplot-iostream.h"
+// #include <armadillo>
+// #include "gnuplot-iostream.h"
+// Note that it has its own header guard to avoid double inclusion.
+
+#ifdef ARMA_INCLUDES
+#ifndef GNUPLOT_ARMADILLO_SUPPORT_LOADED
+#define GNUPLOT_ARMADILLO_SUPPORT_LOADED
+namespace gnuplotio {
+
+template <typename T> struct dont_treat_as_stl_container<arma::Row <T> > { typedef boost::mpl::bool_<true> type; };
+template <typename T> struct dont_treat_as_stl_container<arma::Col <T> > { typedef boost::mpl::bool_<true> type; };
+template <typename T> struct dont_treat_as_stl_container<arma::Mat <T> > { typedef boost::mpl::bool_<true> type; };
+template <typename T> struct dont_treat_as_stl_container<arma::Cube <T> > { typedef boost::mpl::bool_<true> type; };
+template <typename T> struct dont_treat_as_stl_container<arma::field<T> > { typedef boost::mpl::bool_<true> type; };
+
+// {{{3 Cube
+
+template <typename T>
+class ArrayTraits<arma::Cube<T> > : public ArrayTraitsDefaults<T> {
+ class SliceRange {
+ public:
+ SliceRange() : p(NULL), col(0), slice(0) { }
+ explicit SliceRange(const arma::Cube<T> *_p, size_t _row, size_t _col) :
+ p(_p), row(_row), col(_col), slice(0) { }
+
+ typedef T value_type;
+ typedef Error_WasNotContainer subiter_type;
+ static const bool is_container = false;
+
+ bool is_end() const { return slice == p->n_slices; }
+
+ void inc() { ++slice; }
+
+ value_type deref() const {
+ return (*p)(row, col, slice);
+ }
+
+ subiter_type deref_subiter() const {
+ GNUPLOT_STATIC_ASSERT_MSG((sizeof(T) == 0), "argument was not a container");
+ throw std::logic_error("static assert should have been triggered by this point");
+ }
+
+ private:
+ const arma::Cube<T> *p;
+ size_t row, col, slice;
+ };
+
+ class ColRange {
+ public:
+ ColRange() : p(NULL), row(0), col(0) { }
+ explicit ColRange(const arma::Cube<T> *_p, size_t _row) :
+ p(_p), row(_row), col(0) { }
+
+ typedef T value_type;
+ typedef SliceRange subiter_type;
+ static const bool is_container = true;
+
+ bool is_end() const { return col == p->n_cols; }
+
+ void inc() { ++col; }
+
+ value_type deref() const {
+ GNUPLOT_STATIC_ASSERT_MSG((sizeof(T) == 0), "can't call deref on an armadillo cube col");
+ throw std::logic_error("static assert should have been triggered by this point");
+ }
+
+ subiter_type deref_subiter() const {
+ return subiter_type(p, row, col);
+ }
+
+ private:
+ const arma::Cube<T> *p;
+ size_t row, col;
+ };
+
+ class RowRange {
+ public:
+ RowRange() : p(NULL), row(0) { }
+ explicit RowRange(const arma::Cube<T> *_p) : p(_p), row(0) { }
+
+ typedef T value_type;
+ typedef ColRange subiter_type;
+ static const bool is_container = true;
+
+ bool is_end() const { return row == p->n_rows; }
+
+ void inc() { ++row; }
+
+ value_type deref() const {
+ GNUPLOT_STATIC_ASSERT_MSG((sizeof(T) == 0), "can't call deref on an armadillo cube row");
+ throw std::logic_error("static assert should have been triggered by this point");
+ }
+
+ subiter_type deref_subiter() const {
+ return subiter_type(p, row);
+ }
+
+ private:
+ const arma::Cube<T> *p;
+ size_t row;
+ };
+
+public:
+ static const bool allow_auto_unwrap = false;
+ static const size_t depth = ArrayTraits<T>::depth + 3;
+
+ typedef RowRange range_type;
+
+ static range_type get_range(const arma::Cube<T> &arg) {
+ //std::cout << arg.n_elem << "," << arg.n_rows << "," << arg.n_cols << std::endl;
+ return range_type(&arg);
+ }
+};
+
+// }}}3
+
+// {{{3 Mat and Field
+
+template <typename RF, typename T>
+class ArrayTraits_ArmaMatOrField : public ArrayTraitsDefaults<T> {
+ class ColRange {
+ public:
+ ColRange() : p(NULL), row(0), col(0) { }
+ explicit ColRange(const RF *_p, size_t _row) :
+ p(_p), row(_row), col(0) { }
+
+ typedef T value_type;
+ typedef Error_WasNotContainer subiter_type;
+ static const bool is_container = false;
+
+ bool is_end() const { return col == p->n_cols; }
+
+ void inc() { ++col; }
+
+ value_type deref() const {
+ return (*p)(row, col);
+ }
+
+ subiter_type deref_subiter() const {
+ GNUPLOT_STATIC_ASSERT_MSG((sizeof(T) == 0), "argument was not a container");
+ throw std::logic_error("static assert should have been triggered by this point");
+ }
+
+ private:
+ const RF *p;
+ size_t row, col;
+ };
+
+ class RowRange {
+ public:
+ RowRange() : p(NULL), row(0) { }
+ explicit RowRange(const RF *_p) : p(_p), row(0) { }
+
+ typedef T value_type;
+ typedef ColRange subiter_type;
+ static const bool is_container = true;
+
+ bool is_end() const { return row == p->n_rows; }
+
+ void inc() { ++row; }
+
+ value_type deref() const {
+ GNUPLOT_STATIC_ASSERT_MSG((sizeof(T) == 0), "can't call deref on an armadillo matrix row");
+ throw std::logic_error("static assert should have been triggered by this point");
+ }
+
+ subiter_type deref_subiter() const {
+ return subiter_type(p, row);
+ }
+
+ private:
+ const RF *p;
+ size_t row;
+ };
+
+public:
+ static const bool allow_auto_unwrap = false;
+ static const size_t depth = ArrayTraits<T>::depth + 2;
+
+ typedef RowRange range_type;
+
+ static range_type get_range(const RF &arg) {
+ //std::cout << arg.n_elem << "," << arg.n_rows << "," << arg.n_cols << std::endl;
+ return range_type(&arg);
+ }
+};
+
+template <typename T>
+class ArrayTraits<arma::field<T> > : public ArrayTraits_ArmaMatOrField<arma::field<T>, T> { };
+
+template <typename T>
+class ArrayTraits<arma::Mat<T> > : public ArrayTraits_ArmaMatOrField<arma::Mat<T>, T> { };
+
+// }}}3
+
+// {{{3 Row
+
+template <typename T>
+class ArrayTraits<arma::Row<T> > : public ArrayTraitsDefaults<T> {
+public:
+ static const bool allow_auto_unwrap = false;
+
+ typedef IteratorRange<typename arma::Row<T>::const_iterator, T> range_type;
+
+ static range_type get_range(const arma::Row<T> &arg) {
+ //std::cout << arg.n_elem << "," << arg.n_rows << "," << arg.n_cols << std::endl;
+ return range_type(arg.begin(), arg.end());
+ }
+};
+
+// }}}3
+
+// {{{3 Col
+
+template <typename T>
+class ArrayTraits<arma::Col<T> > : public ArrayTraitsDefaults<T> {
+public:
+ static const bool allow_auto_unwrap = false;
+
+ typedef IteratorRange<typename arma::Col<T>::const_iterator, T> range_type;
+
+ static range_type get_range(const arma::Col<T> &arg) {
+ //std::cout << arg.n_elem << "," << arg.n_rows << "," << arg.n_cols << std::endl;
+ return range_type(arg.begin(), arg.end());
+ }
+};
+
+// }}}3
+
+} // namespace gnuplotio
+#endif // GNUPLOT_ARMADILLO_SUPPORT_LOADED
+#endif // ARMA_INCLUDES
+
+// }}}2
+
+// }}}1
diff --git a/EXTERNAL/mcqd/COPYING b/EXTERNAL/mcqd/COPYING
new file mode 100644
index 0000000000000000000000000000000000000000..3d90694ade2af6f840b175a00477c8a15cfc6eeb
--- /dev/null
+++ b/EXTERNAL/mcqd/COPYING
@@ -0,0 +1,674 @@
+ GNU GENERAL PUBLIC LICENSE
+ Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+ Preamble
+
+ The GNU General Public License is a free, copyleft license for
+software and other kinds of works.
+
+ 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
+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.
+
+ When we speak of free software, we are referring to freedom, not
+price. Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+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.
+
+ The precise terms and conditions for copying, distribution and
+modification follow.
+
+ TERMS AND CONDITIONS
+
+ 0. Definitions.
+
+ "This License" refers to version 3 of the GNU General Public License.
+
+ "Copyright" also means copyright-like laws that apply to other kinds of
+works, such as semiconductor masks.
+
+ "The Program" refers to any copyrightable work licensed under this
+License. Each licensee is addressed as "you". "Licensees" and
+"recipients" may be individuals or organizations.
+
+ To "modify" a work means to copy from or adapt all or part of the work
+in a fashion requiring copyright permission, other than the making of an
+exact copy. The resulting work is called a "modified version" of the
+earlier work or a work "based on" the earlier work.
+
+ A "covered work" means either the unmodified Program or a work based
+on the Program.
+
+ To "propagate" a work means to do anything with it that, without
+permission, would make you directly or secondarily liable for
+infringement under applicable copyright law, except executing it on a
+computer or modifying a private copy. Propagation includes copying,
+distribution (with or without modification), making available to the
+public, and in some countries other activities as well.
+
+ To "convey" a work means any kind of propagation that enables other
+parties to make or receive copies. Mere interaction with a user through
+a computer network, with no transfer of a copy, is not conveying.
+
+ An interactive user interface displays "Appropriate Legal Notices"
+to the extent that it includes a convenient and prominently visible
+feature that (1) displays an appropriate copyright notice, and (2)
+tells the user that there is no warranty for the work (except to the
+extent that warranties are provided), that licensees may convey the
+work under this License, and how to view a copy of this License. If
+the interface presents a list of user commands or options, such as a
+menu, a prominent item in the list meets this criterion.
+
+ 1. Source Code.
+
+ The "source code" for a work means the preferred form of the work
+for making modifications to it. "Object code" means any non-source
+form of a work.
+
+ A "Standard Interface" means an interface that either is an official
+standard defined by a recognized standards body, or, in the case of
+interfaces specified for a particular programming language, one that
+is widely used among developers working in that language.
+
+ The "System Libraries" of an executable work include anything, other
+than the work as a whole, that (a) is included in the normal form of
+packaging a Major Component, but which is not part of that Major
+Component, and (b) serves only to enable use of the work with that
+Major Component, or to implement a Standard Interface for which an
+implementation is available to the public in source code form. A
+"Major Component", in this context, means a major essential component
+(kernel, window system, and so on) of the specific operating system
+(if any) on which the executable work runs, or a compiler used to
+produce the work, or an object code interpreter used to run it.
+
+ The "Corresponding Source" for a work in object code form means all
+the source code needed to generate, install, and (for an executable
+work) run the object code and to modify the work, including scripts to
+control those activities. However, it does not include the work's
+System Libraries, or general-purpose tools or generally available free
+programs which are used unmodified in performing those activities but
+which are not part of the work. For example, Corresponding Source
+includes interface definition files associated with source files for
+the work, and the source code for shared libraries and dynamically
+linked subprograms that the work is specifically designed to require,
+such as by intimate data communication or control flow between those
+subprograms and other parts of the work.
+
+ The Corresponding Source need not include anything that users
+can regenerate automatically from other parts of the Corresponding
+Source.
+
+ The Corresponding Source for a work in source code form is that
+same work.
+
+ 2. Basic Permissions.
+
+ All rights granted under this License are granted for the term of
+copyright on the Program, and are irrevocable provided the stated
+conditions are met. This License explicitly affirms your unlimited
+permission to run the unmodified Program. The output from running a
+covered work is covered by this License only if the output, given its
+content, constitutes a covered work. This License acknowledges your
+rights of fair use or other equivalent, as provided by copyright law.
+
+ You may make, run and propagate covered works that you do not
+convey, without conditions so long as your license otherwise remains
+in force. You may convey covered works to others for the sole purpose
+of having them make modifications exclusively for you, or provide you
+with facilities for running those works, provided that you comply with
+the terms of this License in conveying all material for which you do
+not control copyright. Those thus making or running the covered works
+for you must do so exclusively on your behalf, under your direction
+and control, on terms that prohibit them from making any copies of
+your copyrighted material outside their relationship with you.
+
+ Conveying under any other circumstances is permitted solely under
+the conditions stated below. Sublicensing is not allowed; section 10
+makes it unnecessary.
+
+ 3. Protecting Users' Legal Rights From Anti-Circumvention Law.
+
+ No covered work shall be deemed part of an effective technological
+measure under any applicable law fulfilling obligations under article
+11 of the WIPO copyright treaty adopted on 20 December 1996, or
+similar laws prohibiting or restricting circumvention of such
+measures.
+
+ When you convey a covered work, you waive any legal power to forbid
+circumvention of technological measures to the extent such circumvention
+is effected by exercising rights under this License with respect to
+the covered work, and you disclaim any intention to limit operation or
+modification of the work as a means of enforcing, against the work's
+users, your or third parties' legal rights to forbid circumvention of
+technological measures.
+
+ 4. Conveying Verbatim Copies.
+
+ You may convey verbatim copies of the Program's source code as you
+receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice;
+keep intact all notices stating that this License and any
+non-permissive terms added in accord with section 7 apply to the code;
+keep intact all notices of the absence of any warranty; and give all
+recipients a copy of this License along with the Program.
+
+ You may charge any price or no price for each copy that you convey,
+and you may offer support or warranty protection for a fee.
+
+ 5. Conveying Modified Source Versions.
+
+ You may convey a work based on the Program, or the modifications to
+produce it from the Program, in the form of source code under the
+terms of section 4, provided that you also meet all of these conditions:
+
+ a) The work must carry prominent notices stating that you modified
+ it, and giving a relevant date.
+
+ b) The work must carry prominent notices stating that it is
+ released under this License and any conditions added under section
+ 7. This requirement modifies the requirement in section 4 to
+ "keep intact all notices".
+
+ c) You must license the entire work, as a whole, under this
+ License to anyone who comes into possession of a copy. This
+ License will therefore apply, along with any applicable section 7
+ additional terms, to the whole of the work, and all its parts,
+ regardless of how they are packaged. This License gives no
+ permission to license the work in any other way, but it does not
+ invalidate such permission if you have separately received it.
+
+ d) If the work has interactive user interfaces, each must display
+ Appropriate Legal Notices; however, if the Program has interactive
+ interfaces that do not display Appropriate Legal Notices, your
+ work need not make them do so.
+
+ A compilation of a covered work with other separate and independent
+works, which are not by their nature extensions of the covered work,
+and which are not combined with it such as to form a larger program,
+in or on a volume of a storage or distribution medium, is called an
+"aggregate" if the compilation and its resulting copyright are not
+used to limit the access or legal rights of the compilation's users
+beyond what the individual works permit. Inclusion of a covered work
+in an aggregate does not cause this License to apply to the other
+parts of the aggregate.
+
+ 6. Conveying Non-Source Forms.
+
+ You may convey a covered work in object code form under the terms
+of sections 4 and 5, provided that you also convey the
+machine-readable Corresponding Source under the terms of this License,
+in one of these ways:
+
+ a) Convey the object code in, or embodied in, a physical product
+ (including a physical distribution medium), accompanied by the
+ Corresponding Source fixed on a durable physical medium
+ customarily used for software interchange.
+
+ b) Convey the object code in, or embodied in, a physical product
+ (including a physical distribution medium), accompanied by a
+ written offer, valid for at least three years and valid for as
+ long as you offer spare parts or customer support for that product
+ model, to give anyone who possesses the object code either (1) a
+ copy of the Corresponding Source for all the software in the
+ product that is covered by this License, on a durable physical
+ medium customarily used for software interchange, for a price no
+ more than your reasonable cost of physically performing this
+ conveying of source, or (2) access to copy the
+ Corresponding Source from a network server at no charge.
+
+ c) Convey individual copies of the object code with a copy of the
+ written offer to provide the Corresponding Source. This
+ alternative is allowed only occasionally and noncommercially, and
+ only if you received the object code with such an offer, in accord
+ with subsection 6b.
+
+ d) Convey the object code by offering access from a designated
+ place (gratis or for a charge), and offer equivalent access to the
+ Corresponding Source in the same way through the same place at no
+ further charge. You need not require recipients to copy the
+ Corresponding Source along with the object code. If the place to
+ copy the object code is a network server, the Corresponding Source
+ may be on a different server (operated by you or a third party)
+ that supports equivalent copying facilities, provided you maintain
+ clear directions next to the object code saying where to find the
+ Corresponding Source. Regardless of what server hosts the
+ Corresponding Source, you remain obligated to ensure that it is
+ available for as long as needed to satisfy these requirements.
+
+ e) Convey the object code using peer-to-peer transmission, provided
+ you inform other peers where the object code and Corresponding
+ Source of the work are being offered to the general public at no
+ charge under subsection 6d.
+
+ A separable portion of the object code, whose source code is excluded
+from the Corresponding Source as a System Library, need not be
+included in conveying the object code work.
+
+ A "User Product" is either (1) a "consumer product", which means any
+tangible personal property which is normally used for personal, family,
+or household purposes, or (2) anything designed or sold for incorporation
+into a dwelling. In determining whether a product is a consumer product,
+doubtful cases shall be resolved in favor of coverage. For a particular
+product received by a particular user, "normally used" refers to a
+typical or common use of that class of product, regardless of the status
+of the particular user or of the way in which the particular user
+actually uses, or expects or is expected to use, the product. A product
+is a consumer product regardless of whether the product has substantial
+commercial, industrial or non-consumer uses, unless such uses represent
+the only significant mode of use of the product.
+
+ "Installation Information" for a User Product means any methods,
+procedures, authorization keys, or other information required to install
+and execute modified versions of a covered work in that User Product from
+a modified version of its Corresponding Source. The information must
+suffice to ensure that the continued functioning of the modified object
+code is in no case prevented or interfered with solely because
+modification has been made.
+
+ If you convey an object code work under this section in, or with, or
+specifically for use in, a User Product, and the conveying occurs as
+part of a transaction in which the right of possession and use of the
+User Product is transferred to the recipient in perpetuity or for a
+fixed term (regardless of how the transaction is characterized), the
+Corresponding Source conveyed under this section must be accompanied
+by the Installation Information. But this requirement does not apply
+if neither you nor any third party retains the ability to install
+modified object code on the User Product (for example, the work has
+been installed in ROM).
+
+ The requirement to provide Installation Information does not include a
+requirement to continue to provide support service, warranty, or updates
+for a work that has been modified or installed by the recipient, or for
+the User Product in which it has been modified or installed. Access to a
+network may be denied when the modification itself materially and
+adversely affects the operation of the network or violates the rules and
+protocols for communication across the network.
+
+ Corresponding Source conveyed, and Installation Information provided,
+in accord with this section must be in a format that is publicly
+documented (and with an implementation available to the public in
+source code form), and must require no special password or key for
+unpacking, reading or copying.
+
+ 7. Additional Terms.
+
+ "Additional permissions" are terms that supplement the terms of this
+License by making exceptions from one or more of its conditions.
+Additional permissions that are applicable to the entire Program shall
+be treated as though they were included in this License, to the extent
+that they are valid under applicable law. If additional permissions
+apply only to part of the Program, that part may be used separately
+under those permissions, but the entire Program remains governed by
+this License without regard to the additional permissions.
+
+ When you convey a copy of a covered work, you may at your option
+remove any additional permissions from that copy, or from any part of
+it. (Additional permissions may be written to require their own
+removal in certain cases when you modify the work.) You may place
+additional permissions on material, added by you to a covered work,
+for which you have or can give appropriate copyright permission.
+
+ Notwithstanding any other provision of this License, for material you
+add to a covered work, you may (if authorized by the copyright holders of
+that material) supplement the terms of this License with terms:
+
+ a) Disclaiming warranty or limiting liability differently from the
+ terms of sections 15 and 16 of this License; or
+
+ b) Requiring preservation of specified reasonable legal notices or
+ author attributions in that material or in the Appropriate Legal
+ Notices displayed by works containing it; or
+
+ c) Prohibiting misrepresentation of the origin of that material, or
+ requiring that modified versions of such material be marked in
+ reasonable ways as different from the original version; or
+
+ d) Limiting the use for publicity purposes of names of licensors or
+ authors of the material; or
+
+ e) Declining to grant rights under trademark law for use of some
+ trade names, trademarks, or service marks; or
+
+ f) Requiring indemnification of licensors and authors of that
+ material by anyone who conveys the material (or modified versions of
+ it) with contractual assumptions of liability to the recipient, for
+ any liability that these contractual assumptions directly impose on
+ those licensors and authors.
+
+ All other non-permissive additional terms are considered "further
+restrictions" within the meaning of section 10. If the Program as you
+received it, or any part of it, contains a notice stating that it is
+governed by this License along with a term that is a further
+restriction, you may remove that term. If a license document contains
+a further restriction but permits relicensing or conveying under this
+License, you may add to a covered work material governed by the terms
+of that license document, provided that the further restriction does
+not survive such relicensing or conveying.
+
+ If you add terms to a covered work in accord with this section, you
+must place, in the relevant source files, a statement of the
+additional terms that apply to those files, or a notice indicating
+where to find the applicable terms.
+
+ Additional terms, permissive or non-permissive, may be stated in the
+form of a separately written license, or stated as exceptions;
+the above requirements apply either way.
+
+ 8. Termination.
+
+ You may not propagate or modify a covered work except as expressly
+provided under this License. Any attempt otherwise to propagate or
+modify it is void, and will automatically terminate your rights under
+this License (including any patent licenses granted under the third
+paragraph of section 11).
+
+ However, if you cease all violation of this License, then your
+license from a particular copyright holder is reinstated (a)
+provisionally, unless and until the copyright holder explicitly and
+finally terminates your license, and (b) permanently, if the copyright
+holder fails to notify you of the violation by some reasonable means
+prior to 60 days after the cessation.
+
+ Moreover, your license from a particular copyright holder is
+reinstated permanently if the copyright holder notifies you of the
+violation by some reasonable means, this is the first time you have
+received notice of violation of this License (for any work) from that
+copyright holder, and you cure the violation prior to 30 days after
+your receipt of the notice.
+
+ Termination of your rights under this section does not terminate the
+licenses of parties who have received copies or rights from you under
+this License. If your rights have been terminated and not permanently
+reinstated, you do not qualify to receive new licenses for the same
+material under section 10.
+
+ 9. Acceptance Not Required for Having Copies.
+
+ You are not required to accept this License in order to receive or
+run a copy of the Program. Ancillary propagation of a covered work
+occurring solely as a consequence of using peer-to-peer transmission
+to receive a copy likewise does not require acceptance. However,
+nothing other than this License grants you permission to propagate or
+modify any covered work. These actions infringe copyright if you do
+not accept this License. Therefore, by modifying or propagating a
+covered work, you indicate your acceptance of this License to do so.
+
+ 10. Automatic Licensing of Downstream Recipients.
+
+ Each time you convey a covered work, the recipient automatically
+receives a license from the original licensors, to run, modify and
+propagate that work, subject to this License. You are not responsible
+for enforcing compliance by third parties with this License.
+
+ An "entity transaction" is a transaction transferring control of an
+organization, or substantially all assets of one, or subdividing an
+organization, or merging organizations. If propagation of a covered
+work results from an entity transaction, each party to that
+transaction who receives a copy of the work also receives whatever
+licenses to the work the party's predecessor in interest had or could
+give under the previous paragraph, plus a right to possession of the
+Corresponding Source of the work from the predecessor in interest, if
+the predecessor has it or can get it with reasonable efforts.
+
+ You may not impose any further restrictions on the exercise of the
+rights granted or affirmed under this License. For example, you may
+not impose a license fee, royalty, or other charge for exercise of
+rights granted under this License, and you may not initiate litigation
+(including a cross-claim or counterclaim in a lawsuit) alleging that
+any patent claim is infringed by making, using, selling, offering for
+sale, or importing the Program or any portion of it.
+
+ 11. Patents.
+
+ A "contributor" is a copyright holder who authorizes use under this
+License of the Program or a work on which the Program is based. The
+work thus licensed is called the contributor's "contributor version".
+
+ A contributor's "essential patent claims" are all patent claims
+owned or controlled by the contributor, whether already acquired or
+hereafter acquired, that would be infringed by some manner, permitted
+by this License, of making, using, or selling its contributor version,
+but do not include claims that would be infringed only as a
+consequence of further modification of the contributor version. For
+purposes of this definition, "control" includes the right to grant
+patent sublicenses in a manner consistent with the requirements of
+this License.
+
+ Each contributor grants you a non-exclusive, worldwide, royalty-free
+patent license under the contributor's essential patent claims, to
+make, use, sell, offer for sale, import and otherwise run, modify and
+propagate the contents of its contributor version.
+
+ In the following three paragraphs, a "patent license" is any express
+agreement or commitment, however denominated, not to enforce a patent
+(such as an express permission to practice a patent or covenant not to
+sue for patent infringement). To "grant" such a patent license to a
+party means to make such an agreement or commitment not to enforce a
+patent against the party.
+
+ If you convey a covered work, knowingly relying on a patent license,
+and the Corresponding Source of the work is not available for anyone
+to copy, free of charge and under the terms of this License, through a
+publicly available network server or other readily accessible means,
+then you must either (1) cause the Corresponding Source to be so
+available, or (2) arrange to deprive yourself of the benefit of the
+patent license for this particular work, or (3) arrange, in a manner
+consistent with the requirements of this License, to extend the patent
+license to downstream recipients. "Knowingly relying" means you have
+actual knowledge that, but for the patent license, your conveying the
+covered work in a country, or your recipient's use of the covered work
+in a country, would infringe one or more identifiable patents in that
+country that you have reason to believe are valid.
+
+ If, pursuant to or in connection with a single transaction or
+arrangement, you convey, or propagate by procuring conveyance of, a
+covered work, and grant a patent license to some of the parties
+receiving the covered work authorizing them to use, propagate, modify
+or convey a specific copy of the covered work, then the patent license
+you grant is automatically extended to all recipients of the covered
+work and works based on it.
+
+ A patent license is "discriminatory" if it does not include within
+the scope of its coverage, prohibits the exercise of, or is
+conditioned on the non-exercise of one or more of the rights that are
+specifically granted under this License. You may not convey a covered
+work if you are a party to an arrangement with a third party that is
+in the business of distributing software, under which you make payment
+to the third party based on the extent of your activity of conveying
+the work, and under which the third party grants, to any of the
+parties who would receive the covered work from you, a discriminatory
+patent license (a) in connection with copies of the covered work
+conveyed by you (or copies made from those copies), or (b) primarily
+for and in connection with specific products or compilations that
+contain the covered work, unless you entered into that arrangement,
+or that patent license was granted, prior to 28 March 2007.
+
+ Nothing in this License shall be construed as excluding or limiting
+any implied license or other defenses to infringement that may
+otherwise be available to you under applicable patent law.
+
+ 12. No Surrender of Others' Freedom.
+
+ If conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License. If you cannot convey a
+covered work so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you may
+not convey it at all. For example, if you agree to terms that obligate you
+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.
+
+ 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
+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.
+
+ 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
+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
+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
+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
+public statement of acceptance of a version permanently authorizes you
+to choose that version for the Program.
+
+ Later license versions may give you additional or different
+permissions. However, no additional obligations are imposed on any
+author or copyright holder as a result of your choosing to follow a
+later version.
+
+ 15. Disclaimer of Warranty.
+
+ THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
+OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
+IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
+ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+ 16. Limitation of Liability.
+
+ IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
+THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
+USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
+DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
+PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
+EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGES.
+
+ 17. Interpretation of Sections 15 and 16.
+
+ If the disclaimer of warranty and limitation of liability provided
+above cannot be given local legal effect according to their terms,
+reviewing courts shall apply local law that most closely approximates
+an absolute waiver of all civil liability in connection with the
+Program, unless a warranty or assumption of liability accompanies a
+copy of the Program in return for a fee.
+
+ END OF TERMS AND CONDITIONS
+
+ How to Apply These Terms to Your New Programs
+
+ If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+ To do so, attach the following notices to the program. It is safest
+to attach them to the start of each source file to most effectively
+state the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+ <one line to give the program's name and a brief idea of what it does.>
+ Copyright (C) <year> <name of author>
+
+ 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
+ (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.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+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:
+
+ <program> Copyright (C) <year> <name of author>
+ 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".
+
+ 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
+<http://www.gnu.org/licenses/>.
+
+ 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
+<http://www.gnu.org/philosophy/why-not-lgpl.html>.
\ No newline at end of file
diff --git a/EXTERNAL/mcqd/README b/EXTERNAL/mcqd/README
new file mode 100644
index 0000000000000000000000000000000000000000..981ffc09b633eb3726832c8307037855cb27b4d2
--- /dev/null
+++ b/EXTERNAL/mcqd/README
@@ -0,0 +1,5 @@
+Compile with: g++ -O3 mcqd.cpp -o mcqd
+Run with: mcqd test.clq
+mcqd.cpp ... example program that shows how to use class implemented in mcqd.h
+mcqd.h ... a class that contains two variants of the MCQD maximum clique algorithm
+test.clq ... an example clique in DIMACS format
diff --git a/EXTERNAL/mcqd/mcqd.cpp b/EXTERNAL/mcqd/mcqd.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..86bd9d99f9d189a8fdde263bd8c57505055fb1c7
--- /dev/null
+++ b/EXTERNAL/mcqd/mcqd.cpp
@@ -0,0 +1,107 @@
+/*
+ Copyright 2007-2012 Janez Konc
+
+ If you use this program, please cite:
+ Janez Konc and Dusanka Janezic. An improved branch and bound algorithm for the
+ maximum clique problem. MATCH Commun. Math. Comput. Chem., 2007, 58, 569-590.
+
+ More information at: http://www.sicmm.org/~konc
+
+ 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
+ (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.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <fstream>
+#include <iostream>
+#include <set>
+#include <string.h>
+#include <map>
+#include <assert.h>
+#include <emotion/3rdparty/mcqd.h>
+
+using namespace std;
+
+void read_dimacs(string name, bool** &conn, int &size) {
+ ifstream f (name.c_str());
+ string buffer;
+ assert(f.is_open());
+ set<int> v;
+ multimap<int,int> e;
+ while (!getline(f, buffer).eof()) {
+ if (buffer[0] == 'e') {
+ int vi, vj;
+ sscanf(buffer.c_str(), "%*c %d %d", &vi, &vj);
+ v.insert(vi);
+ v.insert(vj);
+ e.insert(make_pair(vi, vj));
+ }
+ }
+// size = v.size() + 1;
+ size = *v.rbegin() + 1;
+ conn = new bool*[size];
+ for (int i=0; i < size; i++) {
+ conn[i] = new bool[size];
+ memset(conn[i], 0, size * sizeof(bool));
+ }
+ for (multimap<int,int>::iterator it = e.begin(); it != e.end(); it++) {
+ conn[it->first][it->second] = true;
+ conn[it->second][it->first] = true;
+ }
+ cout << "|E| = " << e.size() << " |V| = " << v.size() << " p = " << (double) e.size() / (v.size() * (v.size() - 1) / 2) << endl;
+ f.close();
+}
+
+
+int main(int argc, char *argv[]) {
+ assert(argc == 2);
+ cout << "args = " << argv[1] << endl;
+ bool **conn;
+ int size;
+ read_dimacs(argv[1], conn, size);
+ cout << "---------- Example 1: run max clique with improved coloring ----------------"<<endl;
+ clock_t start1 = time(NULL);
+ clock_t start2 = clock();
+ Maxclique m(conn, size);
+ int *qmax;
+ int qsize;
+ m.mcq(qmax, qsize); // run max clique with improved coloring
+ cout << "Maximum clique: ";
+ for (int i = 0; i < qsize; i++)
+ cout << qmax[i] << " ";
+ cout << endl;
+ cout << "Size = " << qsize << endl;
+ cout << "Number of steps = " << m.steps() << endl;
+ cout << "Time = " << difftime(time(NULL), start1) << endl;
+ cout << "Time (precise) = " << ((double) (clock() - start2)) / CLOCKS_PER_SEC << endl << endl;
+ delete [] qmax;
+ cout << "---------- Example 2: run max clique with improved coloring and dynamic sorting of vertices ----------------"<<endl;
+ start1 = time(NULL);
+ start2 = clock();
+ Maxclique md(conn, size, 0.025); //(3rd parameter is optional - default is 0.025 - this heuristics parameter enables you to use dynamic resorting of vertices (time expensive)
+ // on the part of the search tree that is close to the root - in this case, approximately 2.5% of the search tree -
+ // you can probably find a more optimal value for your graphs
+ md.mcqdyn(qmax, qsize); // run max clique with improved coloring and dynamic sorting of vertices
+ cout << "Maximum clique: ";
+ for (int i = 0; i < qsize; i++)
+ cout << qmax[i] << " ";
+ cout << endl;
+ cout << "Size = " << qsize << endl;
+ cout << "Number of steps = " << md.steps() << endl;
+ cout << "Time = " << difftime(time(NULL), start1) << endl;
+ cout << "Time (precise) = " << ((double) (clock() - start2)) / CLOCKS_PER_SEC << endl << endl;
+ delete [] qmax;
+ for (int i=0;i<size;i++)
+ delete [] conn[i];
+ delete [] conn;
+ return 0;
+}
diff --git a/EXTERNAL/mcqd/mcqd.h b/EXTERNAL/mcqd/mcqd.h
new file mode 100644
index 0000000000000000000000000000000000000000..ca1ebd8c58a609e9501a72926674bf5354250055
--- /dev/null
+++ b/EXTERNAL/mcqd/mcqd.h
@@ -0,0 +1,295 @@
+/*
+ Copyright 2007-2012 Janez Konc
+
+ If you use this program, please cite:
+ Janez Konc and Dusanka Janezic. An improved branch and bound algorithm for the
+ maximum clique problem. MATCH Commun. Math. Comput. Chem., 2007, 58, 569-590.
+
+ More information at: http://www.sicmm.org/~konc
+
+ 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
+ (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.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef MCQD
+#define MCQD
+
+#include <iostream>
+#include <algorithm>
+#include <assert.h>
+#ifdef DBG
+using namespace std;
+#endif
+
+class Maxclique {
+ const bool* const* e;
+ int pk, level;
+ const float Tlimit;
+ class Vertices {
+ class Vertex {
+ int i, d;
+ public:
+ void set_i(const int ii) { i = ii; }
+ int get_i() const { return i; }
+ void set_degree(int dd) { d = dd; }
+ int get_degree() const { return d; }
+ };
+ Vertex *v;
+ int sz;
+ static bool desc_degree(const Vertex vi, const Vertex vj) { return (vi.get_degree() > vj.get_degree()); }
+ public:
+#ifdef DBG
+ void dbg_v(const string msg="") const {
+ std::cout << msg << " Vertices: [";
+ for (int i=0; i < sz; i++)
+ std::cout << "(" << v[i].get_i() << "," << v[i].get_degree() << ") ";
+ std::cout << "]" << std::endl;
+ }
+#endif
+ Vertices(int size) : sz(0) { v = new Vertex[size]; }
+ ~Vertices () {}
+ void dispose() { if (v) delete [] v; }
+ void sort() { std::sort(v, v+sz, desc_degree); }
+ void init_colors();
+ void set_degrees(Maxclique&);
+ int size() const { return sz; }
+ void push(const int ii) { v[sz++].set_i(ii); };
+ void pop() { sz--; };
+ Vertex& at(const int ii) const { return v[ii]; };
+ Vertex& end() const { return v[sz - 1]; };
+ };
+ class ColorClass {
+ int *i;
+ int sz;
+ public:
+#ifdef DBG
+ void dbg_i(const string msg="") const {
+ std::cout << msg << " Class: [";
+ for (int ii=0; ii < sz; ii++)
+ std::cout << i[ii] << " ";
+ std::cout << "]" << std::endl;
+ }
+#endif
+ ColorClass() : sz(0), i(0) {}
+ ColorClass(const int sz) : sz(sz), i(0) { init(sz); }
+ ~ColorClass() { if (i) delete [] i;
+ }
+ void init(const int sz) { i = new int[sz]; rewind(); }
+ void push(const int ii) { i[sz++] = ii; };
+ void pop() { sz--; };
+ void rewind() { sz = 0; };
+ int size() const { return sz; }
+ int& at(const int ii) const { return i[ii]; }
+ ColorClass& operator=(const ColorClass& dh) {
+ for (int j = 0; j < dh.sz; j++) i[j] = dh.i[j];
+ sz = dh.sz;
+ return *this;
+ }
+ };
+ Vertices V;
+ ColorClass *C, QMAX, Q;
+ class StepCount {
+ int i1, i2;
+ public:
+ StepCount() : i1(0), i2(0) {}
+ void set_i1(const int ii) { i1 = ii; }
+ int get_i1() const { return i1; }
+ void set_i2(const int ii) { i2 = ii; }
+ int get_i2() const { return i2; }
+ void inc_i1() { i1++; }
+ };
+ StepCount *S;
+ bool connection(const int i, const int j) const { return e[i][j]; }
+ bool cut1(const int, const ColorClass&);
+ void cut2(const Vertices&, Vertices&);
+ void color_sort(Vertices&);
+ void expand(Vertices);
+ void expand_dyn(Vertices);
+ void _mcq(int*&, int&, bool);
+ void degree_sort(Vertices &R) { R.set_degrees(*this); R.sort(); }
+public:
+#ifdef DBG
+ void dbg_C() const {
+ for (int i=0; i < V.size(); i++) {
+ std::cout << "C["<< i << "] : ";
+ C[i].dbg_i();
+ }
+ }
+ void dbg_conn() const {
+ for (int i=0; i < V.size(); i++) {
+ for (int j=0; j < V.size(); j++) {
+ std::cout <<e[i][j];
+ }
+ std::cout<< std::endl;
+ }
+ }
+#endif
+ Maxclique(const bool* const*, const int, const float=0.025);
+ int steps() const { return pk; }
+ void mcq(int* &maxclique, int &sz) { _mcq(maxclique, sz, false); }
+ void mcqdyn(int* &maxclique, int &sz) { _mcq(maxclique, sz, true); }
+ ~Maxclique() {
+ if (C) delete [] C;
+ if (S) delete [] S;
+ V.dispose();
+ };
+};
+
+Maxclique::Maxclique (const bool* const* conn, const int sz, const float tt) : pk(0), level(1), Tlimit(tt), V(sz), Q(sz), QMAX(sz) {
+ assert(conn!=0 && sz>0);
+ for (int i=0; i < sz; i++) V.push(i);
+ e = conn;
+ C = new ColorClass[sz + 1];
+ for (int i=0; i < sz + 1; i++) C[i].init(sz + 1);
+ S = new StepCount[sz + 1];
+}
+
+void Maxclique::_mcq(int* &maxclique, int &sz, bool dyn) {
+ V.set_degrees(*this);
+ V.sort();
+ V.init_colors();
+ if (dyn) {
+ for (int i=0; i < V.size() + 1; i++) {
+ S[i].set_i1(0);
+ S[i].set_i2(0);
+ }
+ expand_dyn(V);
+ }
+ else
+ expand(V);
+ maxclique = new int[QMAX.size()];
+ for (int i=0; i<QMAX.size(); i++) {
+ maxclique[i] = QMAX.at(i);
+ }
+ sz = QMAX.size();
+}
+
+void Maxclique::Vertices::init_colors() {
+ const int max_degree = v[0].get_degree();
+ for (int i = 0; i < max_degree; i++)
+ v[i].set_degree(i + 1);
+ for (int i = max_degree; i < sz; i++)
+ v[i].set_degree(max_degree + 1);
+}
+
+void Maxclique::Vertices::set_degrees(Maxclique &m) {
+ for (int i=0; i < sz; i++) {
+ int d = 0;
+ for (int j=0; j < sz; j++)
+ if (m.connection(v[i].get_i(), v[j].get_i())) d++;
+ v[i].set_degree(d);
+ }
+}
+
+bool Maxclique::cut1(const int pi, const ColorClass &A) {
+ for (int i = 0; i < A.size(); i++)
+ if (connection(pi, A.at(i)))
+ return true;
+ return false;
+}
+
+void Maxclique::cut2(const Vertices &A, Vertices &B) {
+ for (int i = 0; i < A.size() - 1; i++) {
+ if (connection(A.end().get_i(), A.at(i).get_i()))
+ B.push(A.at(i).get_i());
+ }
+}
+
+void Maxclique::color_sort(Vertices &R) {
+ int j = 0;
+ int maxno = 1;
+ int min_k = QMAX.size() - Q.size() + 1;
+ C[1].rewind();
+ C[2].rewind();
+ int k = 1;
+ for (int i=0; i < R.size(); i++) {
+ int pi = R.at(i).get_i();
+ k = 1;
+ while (cut1(pi, C[k]))
+ k++;
+ if (k > maxno) {
+ maxno = k;
+ C[maxno + 1].rewind();
+ }
+ C[k].push(pi);
+ if (k < min_k) {
+ R.at(j++).set_i(pi);
+ }
+ }
+ if (j > 0) R.at(j-1).set_degree(0);
+ if (min_k <= 0) min_k = 1;
+ for (k = min_k; k <= maxno; k++)
+ for (int i = 0; i < C[k].size(); i++) {
+ R.at(j).set_i(C[k].at(i));
+ R.at(j++).set_degree(k);
+ }
+}
+
+void Maxclique::expand(Vertices R) {
+ while (R.size()) {
+ if (Q.size() + R.end().get_degree() > QMAX.size()) {
+ Q.push(R.end().get_i());
+ Vertices Rp(R.size());
+ cut2(R, Rp);
+ if (Rp.size()) {
+ color_sort(Rp);
+ pk++;
+ expand(Rp);
+ }
+ else if (Q.size() > QMAX.size()) {
+ //std::cout << "step = " << pk << " current max. clique size = " << Q.size() << std::endl;
+ QMAX = Q;
+ }
+ Rp.dispose();
+ Q.pop();
+ }
+ else {
+ return;
+ }
+ R.pop();
+ }
+}
+
+void Maxclique::expand_dyn(Vertices R) {
+ S[level].set_i1(S[level].get_i1() + S[level - 1].get_i1() - S[level].get_i2());
+ S[level].set_i2(S[level - 1].get_i1());
+ while (R.size()) {
+ if (Q.size() + R.end().get_degree() > QMAX.size()) {
+ Q.push(R.end().get_i());
+ Vertices Rp(R.size());
+ cut2(R, Rp);
+ if (Rp.size()) {
+ if ((float)S[level].get_i1()/++pk < Tlimit) {
+ degree_sort(Rp);
+ }
+ color_sort(Rp);
+ S[level].inc_i1();
+ level++;
+ expand_dyn(Rp);
+ level--;
+ }
+ else if (Q.size() > QMAX.size()) {
+ //std::cout << "step = " << pk << " current max. clique size = " << Q.size() << std::endl;
+ QMAX = Q;
+ }
+ Rp.dispose();
+ Q.pop();
+ }
+ else {
+ return;
+ }
+ R.pop();
+ }
+}
+
+#endif
diff --git a/cmake_modules/FindEigen3.cmake b/cmake_modules/FindEigen3.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..8c2a734c3d3fe5f73fd3d87557037522de13e964
--- /dev/null
+++ b/cmake_modules/FindEigen3.cmake
@@ -0,0 +1,87 @@
+# - Try to find Eigen3 lib
+#
+# This module supports requiring a minimum version, e.g. you can do
+# find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
+#
+# Once done this will define
+#
+# EIGEN3_FOUND - system has eigen lib with correct version
+# EIGEN3_INCLUDE_DIR - the eigen include directory
+# EIGEN3_VERSION - eigen version
+
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
+# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen3_FIND_VERSION)
+ if(NOT Eigen3_FIND_VERSION_MAJOR)
+ set(Eigen3_FIND_VERSION_MAJOR 2)
+ endif(NOT Eigen3_FIND_VERSION_MAJOR)
+ if(NOT Eigen3_FIND_VERSION_MINOR)
+ set(Eigen3_FIND_VERSION_MINOR 91)
+ endif(NOT Eigen3_FIND_VERSION_MINOR)
+ if(NOT Eigen3_FIND_VERSION_PATCH)
+ set(Eigen3_FIND_VERSION_PATCH 0)
+ endif(NOT Eigen3_FIND_VERSION_PATCH)
+
+ set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+endif(NOT Eigen3_FIND_VERSION)
+
+macro(_eigen3_check_version)
+ file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+ string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+ set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+ string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+ set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+ string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+ set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+ set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+ if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+ set(EIGEN3_VERSION_OK FALSE)
+ else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+ set(EIGEN3_VERSION_OK TRUE)
+ endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+
+ if(NOT EIGEN3_VERSION_OK)
+
+ message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+ "but at least version ${Eigen3_FIND_VERSION} is required")
+ endif(NOT EIGEN3_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN3_INCLUDE_DIR)
+
+ # in cache already
+ _eigen3_check_version()
+ set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
+
+else (EIGEN3_INCLUDE_DIR)
+
+ # specific additional paths for some OS
+ if (WIN32)
+ set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include")
+ endif(WIN32)
+
+ find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+ PATHS
+ ${CMAKE_INSTALL_PREFIX}/include
+ ${EIGEN_ADDITIONAL_SEARCH_PATHS}
+ ${KDE4_INCLUDE_DIR}
+ PATH_SUFFIXES eigen3 eigen
+ )
+
+ if(EIGEN3_INCLUDE_DIR)
+ _eigen3_check_version()
+ endif(EIGEN3_INCLUDE_DIR)
+
+ include(FindPackageHandleStandardArgs)
+ find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+
+ mark_as_advanced(EIGEN3_INCLUDE_DIR)
+
+endif(EIGEN3_INCLUDE_DIR)
+
diff --git a/cmake_modules/falkolibConfig.cmake b/cmake_modules/falkolibConfig.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..750b29ee9667404cd0bca0ec4e1c91bfb6d4d95b
--- /dev/null
+++ b/cmake_modules/falkolibConfig.cmake
@@ -0,0 +1,41 @@
+# - Try to find Library falkolib
+# Once done, this will define
+#
+# falkolib_FOUND - system has falkolib module
+# falkolib_INCLUDE_DIRS - the falkolib include directories
+# falkolib_LIBRARY_DIRS - the falkolib library directories
+# falkolib_LIBRARIES - link these to use falkolib
+
+
+# Uses directory to search mrf_segmentation directory!
+set(falkolib_PREFIX_DIR /usr/local)
+message(STATUS "Searching falkolib in directory ${falkolib_PREFIX_DIR}." )
+
+# Searches include directory /usr/local/include/falkolib
+find_path(falkolib_INCLUDE_DIR falkolib ${falkolib_PREFIX_DIR}/include)
+message(STATUS " falkolib_INCLUDE_DIR ${falkolib_INCLUDE_DIR}." )
+set(falkolib_INCLUDE_DIRS ${falkolib_INCLUDE_DIR})
+
+# Searches library librimagraph.a in /usr/local/lib
+find_path(falkolib_LIBRARY_DIR librimagraph.a ${falkolib_PREFIX_DIR}/lib)
+message(STATUS " falkolib_LIBRARY_DIR ${falkolib_LIBRARY_DIR}." )
+set(falkolib_LIBRARY_DIRS ${falkolib_PREFIX_DIR}/lib)
+
+# Sets the names of library components (actually A name and A component)
+find_library(falkolib_LIBRARY falkolib ${falkolib_LIBRARY_DIRS})
+message(STATUS " falkolib_LIBRARY ${falkolib_LIBRARY}." )
+set(falkolib_LIBRARIES ${falkolib_LIBRARY})
+
+if(("${falkolib_INCLUDE_DIR}" STREQUAL "falkolib_INCLUDE_DIR-NOTFOUND") OR
+ ("${falkolib_LIBRARY_DIRS}" STREQUAL "falkolib_LIBRARY_DIRS-NOTFOUND") OR
+ ("${falkolib_LIBRARY}" STREQUAL "falkolib_LIBRARY-NOTFOUND")
+ )
+ message(STATUS "Library falkolib NOT found")
+ unset(falkolib_FOUND)
+ unset(falkolib_INCLUDE_DIR)
+ unset(falkolib_LIBRARY_DIR)
+ unset(falkolib_LIBRARY)
+ unset(falkolib_LIBRARIES)
+endif()
+
+mark_as_advanced(falkolib_INCLUDE_DIRS falkolib_LIBRARY_DIRS falkolib_LIBRARIES)
diff --git a/include/falkolib/Common/GeomUtils.h b/include/falkolib/Common/GeomUtils.h
new file mode 100644
index 0000000000000000000000000000000000000000..ddb445e5471e9ac1cd72b766199b359bcad2b088
--- /dev/null
+++ b/include/falkolib/Common/GeomUtils.h
@@ -0,0 +1,114 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+/**
+ * @brief Useful geometric functions
+ */
+
+#include <falkolib/Common/Point.h>
+
+#include <Eigen/Dense>
+
+namespace falkolib {
+
+ /**
+ * @brief point-to-point distance
+ * @param p1 point 1
+ * @param p2 point 2
+ * @return distance between p1 and p2F
+ */
+ template <typename T>
+ double pointsDistance(const T& p1, const T& p2) {
+ return (p2 - p1).norm();
+ }
+
+ /**
+ * @brief angle between two given points
+ * @param p1 point 1
+ * @param p2 point 2
+ * @return angle between p1 and p2 [rad]
+ */
+ template <typename T>
+ double angleBetweenPoints(const T& p1, const T& p2) {
+ double angle = atan2(p2[1] - p1[1], p2[0] - p1[0]);
+ //numeric problems..........
+ if (angle >= M_PI) return M_PI - 0.000001;
+ if (angle <= -M_PI) return -M_PI + 0.000001;
+ return angle;
+ }
+
+ /**
+ * @brief This function computes the inner area of a triangle defined by 3 vertices
+ * @param p0 triangle first vertex
+ * @param p1 triangle second vertex
+ * @param p2 triangle third vertex
+ * @return signed area of the inner triangle, p0-p1 as base of the triangle
+ */
+ template <typename T>
+ double signedTriangleArea(const T& p0, const T& p1, const T& p2) {
+ return ((p2[1] - p1[1]) * p0[0] - (p2[0] - p1[0]) * p0[1] + p2[0] * p1[1] - p2[1] * p1[0]);
+ }
+
+ /**
+ * @brief Compute Affine transform between two points sets using least square regression
+ * @param v1 first points set
+ * @param v2 second points set
+ * @param indices matching vectors, pair.first corresponds to v1 and pair.second corresponds to v2
+ * @param transform resulting affine transform which move v2 in v1 frame.
+ * @return if false, the resulting transform is invalid
+ */
+ template <typename T>
+ bool computeTransform(const std::vector<T>& v1, const std::vector<T>& v2, const std::vector<std::pair<int, int> >& indices, Eigen::Affine2d& transform) {
+
+ Eigen::Vector2d t1 = Eigen::Vector2d::Zero();
+ Eigen::Vector2d t2 = Eigen::Vector2d::Zero();
+ Eigen::Matrix2d S = Eigen::Matrix2d::Zero();
+ int n = 0;
+ for (int i = 0; i < (int) indices.size(); ++i) {
+ if (0 <= indices[i].first && indices[i].first < (int) v1.size() &&
+ 0 <= indices[i].second && indices[i].second < (int) v2.size()) {
+ t1 += v1[indices[i].first].point;
+ t2 += v2[indices[i].second].point;
+ n++;
+ }
+ }
+ if (n == 0) {
+ return false;
+ }
+ t1 = (1.0 / n) * t1;
+ t2 = (1.0 / n) * t2;
+ for (int i = 0; i < (int) indices.size(); ++i) {
+ if (0 <= indices[i].first && indices[i].first < (int) v1.size() &&
+ 0 <= indices[i].second && indices[i].second < (int) v2.size()) {
+ S += (v2[indices[i].second].point - t2) * ((v1[indices[i].first].point - t1).transpose());
+ }
+ }
+ double theta = std::atan2(S(0, 1) - S(1, 0), S(0, 0) + S(1, 1));
+ Eigen::Rotation2Dd rot(theta);
+ Eigen::Vector2d transl = t1 - (rot * t2);
+ transform = Eigen::Affine2d::Identity();
+ transform.prerotate(rot);
+ transform.pretranslate(transl);
+
+ return true;
+ }
+
+}
\ No newline at end of file
diff --git a/include/falkolib/Common/HoughSpectrum.h b/include/falkolib/Common/HoughSpectrum.h
new file mode 100644
index 0000000000000000000000000000000000000000..c95bff5c35107ad0f940b2c094f9d3b401241e25
--- /dev/null
+++ b/include/falkolib/Common/HoughSpectrum.h
@@ -0,0 +1,147 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <iostream>
+#include <Eigen/Dense>
+
+namespace falkolib {
+
+ /** Hough transform and spectrum.
+ */
+ class HoughSpectrum {
+ public:
+ /** Constructor with deafult parameters.
+ */
+ HoughSpectrum();
+
+ /** Constructor with the number of theta.
+ */
+ HoughSpectrum(double thetaStep, double rhoStep, double rhoMax);
+
+ /** Inits params.
+ */
+ void init(double thetaStep, double rhoStep, double rhoMax);
+
+ /** Inserts the points and computes Hough Transform and Spectrum.
+ */
+ template <typename It>
+ void insertPoint(It pbeg, It pend);
+
+ /** Returns the Hough Transform.
+ */
+ const Eigen::MatrixXd& hough() const {
+ return hough_;
+ }
+
+ /** Returns the value of Hough Transform for a specific value of theta and rho.
+ * If the theta and rho are not in the domain, then it return 0.0.
+ */
+ double hough(double theta, double rho) const {
+ int ith = thetaToIdx(theta);
+ int irh = rhoToIdx(theta);
+ if (0 <= ith && ith < hough_.rows() && 0 <= irh && irh < hough_.cols()) {
+ return hough_(ith, irh);
+ }
+ return 0.0;
+ }
+
+ /** Returns the spectrum.
+ */
+ const Eigen::VectorXd& spectrum() const {
+ return spectrum_;
+ }
+
+ /** Returns the spectrum.
+ */
+ const double spectrum(double theta) const {
+ int ith = thetaToIdx(theta);
+ if (0 <= ith && ith < hough_.rows()) {
+ return spectrum_(ith);
+ }
+ return 0.0;
+ }
+
+ /** Returns the spectrum.
+ */
+ const Eigen::VectorXd& orthoSpectrum() const {
+ return orthoSpectrum_;
+ }
+
+ private:
+ int thetaNum_;
+ int rhoNum_;
+ double thetaStep_;
+ double rhoStep_;
+ // Hough transform and spectra should be integer types.
+ // However, since their value may be very large, double type is used instead.
+ Eigen::MatrixXd hough_;
+ Eigen::VectorXd spectrum_;
+ Eigen::VectorXd orthoSpectrum_;
+ Eigen::VectorXd cosLut_;
+ Eigen::VectorXd sinLut_;
+
+ double idxToRho(int idx) const {
+ return (rhoStep_ * (idx - rhoNum_));
+ }
+
+ int rhoToIdx(double rho) const {
+ return ((int) round(rho / rhoStep_) + rhoNum_ / 2);
+ }
+
+ int thetaToIdx(double theta) const {
+ int idx = (int) round(theta / thetaStep_);
+ int thetaNum2 = 2 * thetaNum_;
+ idx = ((idx % thetaNum2) + thetaNum2) % thetaNum2;
+ return idx;
+ }
+ };
+
+ // -------------------------------------------------------------
+ // TEMPLATE METHOD
+ // -------------------------------------------------------------
+
+ template <typename It>
+ void HoughSpectrum::insertPoint(It pbeg, It pend) {
+ double rho;
+ int irho;
+
+ hough_.fill(0.0);
+ spectrum_.fill(0.0);
+ // Computes Hough
+ for (It pit = pbeg; pit != pend; ++pit) {
+ for (int i = 0; i < thetaNum_; ++i) {
+ rho = pit->x() * cosLut_(i) + pit->y() * sinLut_(i);
+ irho = rhoToIdx(rho);
+ if (0 <= irho && irho < rhoNum_) {
+ hough_(i, irho) = hough_(i, irho) + 1;
+ }
+ // else {
+ // std::cerr << "Out-of-bound: rho " << rho << ", theta " << (180.0 / M_PI * thetaStep_ * i) << ", "
+ // << "point [" << pit->x() << " " << pit->y() << "]\n";
+ // }
+ }
+ }
+ spectrum_ = (hough_.array() * hough_.array()).rowwise().sum();
+ orthoSpectrum_ = spectrum_.segment(0, thetaNum_ / 2) + spectrum_.segment(thetaNum_ / 2, thetaNum_ / 2);
+ }
+
+} // end of namespace
+
diff --git a/include/falkolib/Common/LaserScan.h b/include/falkolib/Common/LaserScan.h
new file mode 100644
index 0000000000000000000000000000000000000000..eae9e962d651be31c35405f3b8611c9e054c83c8
--- /dev/null
+++ b/include/falkolib/Common/LaserScan.h
@@ -0,0 +1,155 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <vector>
+#include <falkolib/Common/Point.h>
+#include <falkolib/Common/GeomUtils.h>
+
+namespace falkolib {
+
+ /**
+ * @brief Laser scan container
+ *
+ * This class provides an essential interface for laser scan data
+ */
+ class LaserScan {
+ public:
+
+ /**
+ * @brief Constructor
+ */
+ LaserScan() {
+ angleMin = 0;
+ fov = 0;
+ angleInc = 0;
+ numBeams = 0;
+ timestamp = 0;
+ }
+
+ /**
+ * @brief Constructor
+ * @param _angleMin laser scanner start angle [rad]
+ * @param _fov laser scanner field of view [rad]
+ * @param _numBeams laser scanner number of beams
+ */
+ LaserScan(double _angleMin, double _fov, int _numBeams) {
+ angleMin = _angleMin;
+ fov = _fov;
+ angleInc = _fov / _numBeams;
+ numBeams = _numBeams;
+ timestamp = 0;
+ }
+
+ /** @brief Set laser scanner start angle [rad] */
+ inline void setAngleMin(double _angleMin) {
+ angleMin = _angleMin;
+ };
+
+ /** @brief Set laser scanner field of view [rad] */
+ inline void setLaserFoV(double _fov) {
+ fov = _fov;
+ };
+
+ /** @brief Set laser scanner angle increment [rad] */
+ inline void setAngleInc(double _angleInc) {
+ angleInc = _angleInc;
+ };
+
+ /** @brief Set laser scanner number of beams */
+ inline void setNumBeams(int _numBeams) {
+ numBeams = _numBeams;
+ };
+
+ /** @brief Set scan beginning timestamp [s] */
+ inline void setTimestamp(double _timestamp) {
+ timestamp = _timestamp;
+ };
+
+ /** @brief Get laser scanner number of beams */
+ inline int getNumBeams() const {
+ return numBeams;
+ };
+
+ /** @brief Get laser scanner angle increment [rad] */
+ inline double getAngleInc() const {
+ return angleInc;
+ };
+
+ /**
+ * @brief Compute scan points from ranges
+ *
+ * @param _ranges plain array of double representing the scan ranges
+ */
+ inline void fromRanges(const double* _ranges) {
+ fromRanges(std::vector<double>(_ranges, _ranges + numBeams));
+ }
+
+ /**
+ * @brief Compute scan points from ranges
+ *
+ * @param _ranges std::vector of double representing the scan ranges
+ */
+ inline void fromRanges(const std::vector<double>& _ranges) {
+ double theta;
+ ranges = _ranges;
+ points.resize(numBeams);
+ for (int i = 0; i < numBeams; ++i) {
+ theta = i * angleInc + angleMin;
+ points[i][0] = ranges[i] * std::cos(theta);
+ points[i][1] = ranges[i] * std::sin(theta);
+ }
+ }
+
+ /**
+ * @brief compute neighborhood points list given a single point index and a search radius
+ * @param candIndex index of the central point
+ * @param radius search euclidean radius [m]
+ * @param neigh vector of the neighborhood points
+ * @param midIndex index representing the central point in the neigh vector
+ */
+ void getNeighPoints(int candIndex, double radius, std::vector<Point2d>& neigh, int& midIndex) const {
+ const Point2d& candPoint = points[candIndex];
+ int alpha = std::floor(std::asin(radius / ranges[candIndex]) / angleInc);
+ int begIndex = std::max(0, candIndex - alpha);
+ int endIndex = std::min(candIndex + alpha + 1, numBeams);
+ for (int i = begIndex; i <= endIndex; ++i) {
+ if (pointsDistance(points[i], candPoint) <= radius) {
+ if (i == candIndex) {
+ midIndex = neigh.size();
+ }
+ neigh.push_back(points[i]);
+ }
+ }
+ }
+
+
+ std::vector<double> ranges;
+ std::vector<Point2d> points;
+
+ private:
+
+ double angleMin;
+ double fov;
+ double angleInc;
+ int numBeams;
+ double timestamp;
+ };
+}
\ No newline at end of file
diff --git a/include/falkolib/Common/Point.h b/include/falkolib/Common/Point.h
new file mode 100644
index 0000000000000000000000000000000000000000..4fb0bd6485e28a311ab137e746f250613567b95c
--- /dev/null
+++ b/include/falkolib/Common/Point.h
@@ -0,0 +1,28 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <Eigen/Core>
+
+namespace falkolib {
+ typedef Eigen::Vector2d Point2d;
+ typedef Eigen::Vector2f Point2f;
+ typedef Eigen::Vector2i Point2i;
+}
\ No newline at end of file
diff --git a/include/falkolib/Feature/BSC.h b/include/falkolib/Feature/BSC.h
new file mode 100644
index 0000000000000000000000000000000000000000..f3403f1070384707fae0be06c65bfcf9c97b9c19
--- /dev/null
+++ b/include/falkolib/Feature/BSC.h
@@ -0,0 +1,81 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <falkolib/Feature/Descriptor.h>
+#include <falkolib/Common/Point.h>
+#include <vector>
+
+namespace falkolib {
+
+ /**
+ * @brief Binary Shape Context descriptor
+ *
+ * This class represents a BSC Descriptor.
+ */
+ class BSC : public Descriptor {
+ public:
+
+ /**
+ * @brief Constructor
+ * @param _radius descriptor max radius
+ * @param _circularSectorNumber number of circular sectors
+ * @param _radialRingNumber number of radial rings
+ *
+ * Set the grid dimension and cells resolutions
+ */
+ BSC(double _radius, int _circularSectorNumber, int _radialRingNumber);
+
+ /**
+ * @brief Compute distance between two descriptors
+ * @param desc descriptor to measure distance
+ * @return the distance between *this and desc
+ *
+ * Compute the distance between two descriptors of the same type (BSC)
+ */
+ double distance(const Descriptor& desc) const;
+
+ /**
+ * @brief Rotate the descriptor grid
+ * @param theta angle of rotation [rad]
+ *
+ * Rotate the descriptor grid of a number of circular sector based on theta
+ */
+ void rotate(double theta);
+
+ /**
+ * @brief Compute the grid descriptor
+ * @param neigh vector of neighborhood points
+ * @param centralPointIndex index of the central point in the neigh vector
+ */
+ void compute(std::vector<Point2d>& neigh, int centralPointIndex);
+
+ private:
+ std::vector<std::vector<uint8_t> > grid;
+ int circularSectorNumber;
+ int radialRingNumber;
+ double sectorResolution;
+ double ringResolution;
+ double radius;
+
+ /** @brief compute the Hamming distance between two binary grid*/
+ double HammingDistance(const std::vector<std::vector<uint8_t> >& g1, const std::vector<std::vector<uint8_t> >& g2) const;
+ };
+}
\ No newline at end of file
diff --git a/include/falkolib/Feature/BSCExtractor.h b/include/falkolib/Feature/BSCExtractor.h
new file mode 100644
index 0000000000000000000000000000000000000000..aee7ce5189dd0e0177496e6a30598bb6375fd3a9
--- /dev/null
+++ b/include/falkolib/Feature/BSCExtractor.h
@@ -0,0 +1,81 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <falkolib/Common/LaserScan.h>
+#include <falkolib/Feature/Keypoint.h>
+#include <falkolib/Feature/DescriptorExtractor.h>
+#include <falkolib/Feature/BSC.h>
+
+namespace falkolib {
+
+ /**
+ * @brief class representing a BSC descriptor extractor engine
+ */
+ template <typename T>
+ class BSCExtractor : public DescriptorExtractor<T, BSC> {
+ public:
+
+ /**
+ * @brief Constructor
+ * @param _circularSectorNumber number of grid circular sector
+ * @param _radialRingNumber number of grid radial number
+ * @param _useKeypointRadius if true, the selected neighborhood points search radius is keypoint one
+ * @param _radius neighborhood points search radius
+ */
+ BSCExtractor(int _circularSectorNumber, int _radialRingNumber, bool _useKeypointRadius = true, double _radius = 0.1) {
+ circularSectorNumber = _circularSectorNumber;
+ radialRingNumber = _radialRingNumber;
+ useKeypointRadius = _useKeypointRadius;
+ radius = _radius;
+ };
+
+ /**
+ * @brief Extract BSC descriptor from a given scan and a list of keypoints
+ * @param scan input laser scan
+ * @param keypoints keypoints list
+ * @param descriptor extracted from scan and keypoints
+ */
+ void compute(const LaserScan& scan, const std::vector<T>& keypoints, std::vector<BSC>& descriptors) {
+ descriptors.reserve(keypoints.size());
+ for (int i = 0; i < keypoints.size(); ++i) {
+ std::vector<Point2d> neigh;
+ int midIndex;
+ if (useKeypointRadius) {
+ scan.getNeighPoints(keypoints[i].index, keypoints[i].radius, neigh, midIndex);
+ BSC desc(keypoints[i].radius, circularSectorNumber, radialRingNumber);
+ desc.compute(neigh, midIndex);
+ descriptors.push_back(std::move(desc));
+ } else {
+ scan.getNeighPoints(keypoints[i].index, radius, neigh, midIndex);
+ BSC desc(radius, circularSectorNumber, radialRingNumber);
+ desc.compute(neigh, midIndex);
+ descriptors.push_back(std::move(desc));
+ }
+ }
+ };
+
+ private:
+ int circularSectorNumber;
+ int radialRingNumber;
+ bool useKeypointRadius;
+ double radius;
+ };
+}
\ No newline at end of file
diff --git a/include/falkolib/Feature/CGH.h b/include/falkolib/Feature/CGH.h
new file mode 100644
index 0000000000000000000000000000000000000000..60e3cecd4f866ccf7cb5f0ce7b5f04d6b7448790
--- /dev/null
+++ b/include/falkolib/Feature/CGH.h
@@ -0,0 +1,78 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <falkolib/Feature/Descriptor.h>
+#include <falkolib/Common/Point.h>
+#include <vector>
+
+namespace falkolib {
+
+ /**
+ * @brief Cumulative Gaussian Histogram descriptor
+ *
+ * This class represents a CGH Descriptor.
+ */
+ class CGH : public Descriptor {
+ public:
+
+ /**
+ * @brief Constructor
+ * @param _radius descriptor max radius
+ * @param _circularSectorNumber number of histogram bins
+ *
+ * Set the histogram dimension and the circular sectors resolutions
+ */
+ CGH(double _radius, int _circularSectorNumber);
+
+ /**
+ * @brief Compute distance between two descriptors
+ * @param desc descriptor to measure distance
+ * @return the distance between *this and desc
+ *
+ * Compute the distance between two descriptors of the same type (CGH)
+ */
+ double distance(const Descriptor& desc) const;
+
+ /**
+ * @brief Rotate the descriptor grid
+ * @param theta angle of rotation [rad]
+ *
+ * Rotate the descriptor grid of a number of circular sector based on theta
+ */
+ void rotate(double theta);
+
+ /**
+ * @brief Compute the histogram descriptor
+ * @param neigh vector of neighborhood points
+ * @param centralPointIndex index of the central point in the neigh vector
+ */
+ void compute(std::vector<Point2d>& neigh, int centralPointIndex);
+
+ private:
+ std::vector<double> histogram;
+ int circularSectorNumber;
+ double sectorResolution;
+ double radius;
+
+ /** @brief compute the Chi-squared distance between two histograms*/
+ double SymmetricChiSquaredDistance(const std::vector<double>& h1, const std::vector<double>& h2) const;
+ };
+}
\ No newline at end of file
diff --git a/include/falkolib/Feature/CGHExtractor.h b/include/falkolib/Feature/CGHExtractor.h
new file mode 100644
index 0000000000000000000000000000000000000000..d8bd7ad9ea0c308574a08115ff23f695163c8170
--- /dev/null
+++ b/include/falkolib/Feature/CGHExtractor.h
@@ -0,0 +1,78 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <falkolib/Common/LaserScan.h>
+#include <falkolib/Feature/Keypoint.h>
+#include <falkolib/Feature/DescriptorExtractor.h>
+#include <falkolib/Feature/CGH.h>
+
+namespace falkolib {
+
+ /**
+ * @brief class representing a CGH descriptor extractor engine
+ */
+ template <typename T>
+ class CGHExtractor : public DescriptorExtractor<T, CGH> {
+ public:
+
+ /**
+ * @brief Constructor
+ * @param _circularSectorNumber number of grid circular sector
+ * @param _useKeypointRadius if true, the selected neighborhood points search radius is keypoint one
+ * @param _radius neighborhood points search radius
+ */
+ CGHExtractor(int _circularSectorNumber, bool _useKeypointRadius = true, double _radius = 0.1) {
+ circularSectorNumber = _circularSectorNumber;
+ useKeypointRadius = _useKeypointRadius;
+ radius = _radius;
+ };
+
+ /**
+ * @brief Extract CGH descriptor from a given scan and a list of keypoints
+ * @param scan input laser scan
+ * @param keypoints keypoints list
+ * @param descriptor extracted from scan and keypoints
+ */
+ void compute(const LaserScan& scan, const std::vector<T>& keypoints, std::vector<CGH>& descriptors) {
+ descriptors.reserve(keypoints.size());
+ for (int i = 0; i < keypoints.size(); ++i) {
+ std::vector<Point2d> neigh;
+ int midIndex;
+ if (useKeypointRadius) {
+ scan.getNeighPoints(keypoints[i].index, keypoints[i].radius, neigh, midIndex);
+ CGH desc(keypoints[i].radius, circularSectorNumber);
+ desc.compute(neigh, midIndex);
+ descriptors.push_back(std::move(desc));
+ } else {
+ scan.getNeighPoints(keypoints[i].index, radius, neigh, midIndex);
+ CGH desc(radius, circularSectorNumber);
+ desc.compute(neigh, midIndex);
+ descriptors.push_back(std::move(desc));
+ }
+ }
+ };
+
+ private:
+ int circularSectorNumber;
+ bool useKeypointRadius;
+ double radius;
+ };
+}
\ No newline at end of file
diff --git a/include/falkolib/Feature/Descriptor.h b/include/falkolib/Feature/Descriptor.h
new file mode 100644
index 0000000000000000000000000000000000000000..530fd14a665eb3a66efc6135e752331253ca524b
--- /dev/null
+++ b/include/falkolib/Feature/Descriptor.h
@@ -0,0 +1,40 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+namespace falkolib {
+
+ /**
+ * @brief class representing a generic descriptor
+ *
+ * No properties are defined for a generic descriptor
+ */
+
+ class Descriptor {
+ protected:
+
+ /**
+ * @brief Compute distance between two descriptors
+ * @param desc descriptor to measure distance
+ * @return the distance between *this and desc
+ */
+ virtual double distance(const Descriptor& desc) const = 0;
+ };
+}
\ No newline at end of file
diff --git a/include/falkolib/Feature/DescriptorExtractor.h b/include/falkolib/Feature/DescriptorExtractor.h
new file mode 100644
index 0000000000000000000000000000000000000000..c1002e44bcd2b990e037c101f7f9751578e7de07
--- /dev/null
+++ b/include/falkolib/Feature/DescriptorExtractor.h
@@ -0,0 +1,41 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <falkolib/Common/LaserScan.h>
+#include <falkolib/Feature/Keypoint.h>
+
+namespace falkolib {
+
+ /**
+ * @brief class representing a descriptor extractor engine
+ */
+ template <typename T, typename D>
+ class DescriptorExtractor {
+ protected:
+ /**
+ * @brief Extract descriptor from a given scan and a list of keypoints
+ * @param scan input laser scan
+ * @param keypoints keypoints list
+ * @param descriptor extracted from scan and keypoints
+ */
+ virtual void compute(const LaserScan& scan, const std::vector<T>& keypoints, std::vector<D>& descriptors) = 0;
+ };
+}
\ No newline at end of file
diff --git a/include/falkolib/Feature/FALKO.h b/include/falkolib/Feature/FALKO.h
new file mode 100644
index 0000000000000000000000000000000000000000..7204b7fcb3f75fc50f7a24f9e144951dd02d4e69
--- /dev/null
+++ b/include/falkolib/Feature/FALKO.h
@@ -0,0 +1,38 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <falkolib/Feature/Keypoint.h>
+
+namespace falkolib{
+
+ /**
+ * @brief class representing a FALKO keypoint
+ *
+ * FALKO keypoint extends simple keypoint interface.
+ * The index in the original scan, corner orientation and neighborhood search radius are also preserved.
+ */
+ class FALKO : public Keypoint{
+ public:
+ int index;
+ double radius;
+ double orientation;
+ };
+}
\ No newline at end of file
diff --git a/include/falkolib/Feature/FALKOExtractor.h b/include/falkolib/Feature/FALKOExtractor.h
new file mode 100644
index 0000000000000000000000000000000000000000..2c50ad3192cafca62f649726251cbb0c8a2dfbaa
--- /dev/null
+++ b/include/falkolib/Feature/FALKOExtractor.h
@@ -0,0 +1,132 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <falkolib/Feature/KeypointExtractor.h>
+#include <falkolib/Feature/FALKO.h>
+#include <falkolib/Common/GeomUtils.h>
+
+namespace falkolib {
+
+ /**
+ * @brief class representing a FALKO keypoint extractor engine
+ */
+ class FALKOExtractor : public KeypointExtractor<FALKO> {
+ public:
+
+ /**
+ * @brief Constructor
+ */
+ FALKOExtractor();
+
+ /**
+ * @brief Extract FALKO keypoints from a given scan
+ * @param scan input laser scan
+ * @param features FALKO keypoint extracted from given scan
+ */
+ void extract(const LaserScan& scan, std::vector<FALKO>& keypoints);
+
+ /** @brief Set minimum score threshold for a candidate keypoint [%]*/
+ inline void setMinScoreTh(double _minScoreTh) {
+ minScoreTh = _minScoreTh;
+ };
+
+ /** @brief Set minimum extraction range [m]*/
+ inline void setMinExtractionRange(double _minExtractionRange) {
+ minExtractionRange = _minExtractionRange;
+ };
+
+ /** @brief Set maximum extraction range [m]*/
+ inline void setMaxExtractionRange(double _maxExtractionRange) {
+ maxExtractionRange = _maxExtractionRange;
+ };
+
+ /** @brief Enable subbeam accuracy in keypoint extraction */
+ inline void enableSubbeam(bool _subbeam) {
+ subbeam = _subbeam;
+ };
+
+ /** @brief Set Non-Maxima-Suppression radius [m]*/
+ inline void setNMSRadius(double _NMSRadius) {
+ NMSRadius = _NMSRadius;
+ };
+
+ /** @brief Set neighA parameter for neighborhood radius computation*/
+ inline void setNeighA(double _neighA) {
+ neighA = _neighA;
+ };
+
+ /** @brief Set neighB parameter for neighborhood radius computation*/
+ inline void setNeighB(double _neighB) {
+ neighB = _neighB;
+ };
+
+ /** @brief Set minimum neighborhood size for each corner side*/
+ inline void setNeighMinPoint(double _neighMinPoint) {
+ neighMinPoint = _neighMinPoint;
+ };
+
+ /** @brief Set b-ratio for geometric corner validation*/
+ inline void setBRatio(double _bRatio) {
+ bRatio = _bRatio;
+ };
+
+ /** @brief Set number of circular grid sector for score computation*/
+ inline void setGridSectors(double _gridSectors) {
+ gridSectors = _gridSectors;
+ };
+
+ private:
+ double minScoreTh;
+ double minExtractionRange;
+ double maxExtractionRange;
+ bool subbeam;
+ double NMSRadius;
+ double neighA;
+ double neighB;
+ int neighMinPoint;
+ double bRatio;
+ int gridSectors;
+
+ /** @brief distance between grid circular sectors*/
+ int circularSectorDistance(int a1, int a2, int res);
+
+ /** @brief return index of corresponding circular sector in the score grid*/
+ int getCircularSectorIndex(const Point2d& p, const Point2d& pmid, double theta);
+
+ /** @brief return neighborhood search radius, different heuristics are used based on rho*/
+ double getNeighRadius(double rho);
+
+ /** @brief compute corner orientation given neighborhood points*/
+ double getCornerOrientation(const std::vector<Point2d>& neigh, int midIndex);
+
+ /** @brief Non-Maxima-Suppression function for keypoint extraction*/
+ void NMSKeypoint(const std::vector<int>& scores, const LaserScan& scan, unsigned int ibeg, unsigned int iend, double radius, int minval, std::vector<int>& peaks);
+
+ /** @brief compute corner position with subbeam accuracy*/
+ void subBeamCorner(const LaserScan& scan, int index, double radius, Point2d& p);
+
+ /** @brief generate a line model using least-square*/
+ void generateLine(const std::vector<Point2d>& points, Eigen::Vector3d& model);
+
+ /** @brief built-in 2x2 system solver function*/
+ bool solveSystem2x2(double* A, double* b, double* x);
+ };
+}
\ No newline at end of file
diff --git a/include/falkolib/Feature/Keypoint.h b/include/falkolib/Feature/Keypoint.h
new file mode 100644
index 0000000000000000000000000000000000000000..00f441f4451cac02a19fd23eca7b3de5834b0294
--- /dev/null
+++ b/include/falkolib/Feature/Keypoint.h
@@ -0,0 +1,44 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <falkolib/Common/Point.h>
+
+namespace falkolib {
+
+ /**
+ * @brief class representing a keypoint
+ *
+ * Keypoints are defined as a 2d point
+ */
+ class Keypoint {
+ public:
+ Point2d point;
+
+ /**
+ * @brief measure distance between keypoints
+ * @param desc external keypoints
+ * @return measured distance
+ */
+ inline double distance(const Keypoint& desc) const {
+ return (point - desc.point).norm();
+ }
+ };
+}
\ No newline at end of file
diff --git a/include/falkolib/Feature/KeypointExtractor.h b/include/falkolib/Feature/KeypointExtractor.h
new file mode 100644
index 0000000000000000000000000000000000000000..d31b7f6539a4e175acb710fbebd90ffb4078b06e
--- /dev/null
+++ b/include/falkolib/Feature/KeypointExtractor.h
@@ -0,0 +1,40 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <falkolib/Common/LaserScan.h>
+#include <falkolib/Feature/Keypoint.h>
+
+namespace falkolib {
+
+ /**
+ * @brief class representing a keypoints extractor engine
+ */
+ template <typename T>
+ class KeypointExtractor {
+ protected:
+ /**
+ * @brief Extract keypoints from a given scan
+ * @param scan input laser scan
+ * @param features keypoints extracted from given scan
+ */
+ virtual void extract(const LaserScan& scan, std::vector<T>& keypoints) = 0;
+ };
+}
\ No newline at end of file
diff --git a/include/falkolib/Feature/OC.h b/include/falkolib/Feature/OC.h
new file mode 100644
index 0000000000000000000000000000000000000000..2bd5768006470e110f8fa55a112489dc7b9509c5
--- /dev/null
+++ b/include/falkolib/Feature/OC.h
@@ -0,0 +1,38 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <falkolib/Feature/Keypoint.h>
+
+namespace falkolib{
+
+ /**
+ * @brief class representing a OC keypoint
+ *
+ * OC keypoint extends simple keypoint interface.
+ * The index in the original scan, corner orientation and neighborhood search radius are also preserved.
+ */
+ class OC : public Keypoint{
+ public:
+ int index;
+ double radius;
+ double orientation;
+ };
+}
diff --git a/include/falkolib/Feature/OCExtractor.h b/include/falkolib/Feature/OCExtractor.h
new file mode 100644
index 0000000000000000000000000000000000000000..27bb351c88c3cb1cfd6a126c5b3749189ce793c1
--- /dev/null
+++ b/include/falkolib/Feature/OCExtractor.h
@@ -0,0 +1,131 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <falkolib/Feature/KeypointExtractor.h>
+#include <falkolib/Feature/OC.h>
+#include <falkolib/Common/Point.h>
+#include <falkolib/Common/GeomUtils.h>
+#include <falkolib/Common/HoughSpectrum.h>
+
+namespace falkolib {
+
+ /**
+ * @brief class representing a OC keypoint extractor engine
+ */
+ class OCExtractor : public KeypointExtractor<OC> {
+ public:
+
+ enum CornerOrientation {
+ NE = 0, NW, SW, SE
+ };
+
+ /**
+ * @brief Constructor
+ */
+ OCExtractor();
+
+ /**
+ * @brief Extract OC keypoints from a given scan
+ * @param scan input laser scan
+ * @param features OC keypoint extracted from given scan
+ */
+ void extract(const LaserScan& scan, std::vector<OC>& keypoints);
+
+ /** @brief Set the tolerance distance to consider point aligned to corner edge. */
+ inline void setTol(double _tol) {
+ tol = _tol;
+ };
+
+ /** @brief Set the angular resolution. */
+ inline void setAngleRes(double _angleRes) {
+ angleRes = _angleRes;
+ houghSpectrum.init(angleRes, rangeRes, rangeMax);
+ };
+
+ /** @brief Set the range/distance resolution used for Hough rho size. */
+ inline void setRangeRes(double _rangeRes) {
+ rangeRes = _rangeRes;
+ houghSpectrum.init(angleRes, rangeRes, rangeMax);
+ };
+
+ /** @brief Set the maximum range for Hough rho cell grid. */
+ inline void setRangeMax(double _rangeMax) {
+ rangeMax = _rangeMax;
+ houghSpectrum.init(angleRes, rangeRes, rangeMax);
+ };
+
+ /** @brief Set the non maxima suppression radius. */
+ inline void setNMSRadius(double _nmsRadius) {
+ nmsRadius = _nmsRadius;
+ };
+
+ /** @brief Set neighA parameter for neighborhood radius computation*/
+ inline void setNeighA(double _neighA) {
+ neighA = _neighA;
+ };
+
+ /** @brief Set neighB parameter for neighborhood radius computation*/
+ inline void setNeighB(double _neighB) {
+ neighB = _neighB;
+ };
+
+ /** @brief Set minimum neighborhood size for each corner side*/
+ inline void setNeighMinPoint(int _neighMinPoint) {
+ neighMinPoint = _neighMinPoint;
+ };
+
+
+ private:
+ HoughSpectrum houghSpectrum;
+ double tol;
+ double angleRes;
+ double rangeRes;
+ double rangeMax;
+ double nmsRadius;
+ double neighA;
+ double neighB;
+ int neighMinPoint;
+
+ /** @brief Computes the dominant direction of a scan using Hough Spectrum.
+ */
+ double computeDominantAngle(const std::vector<Point2d>& points);
+
+ /** @brief Rotates points.
+ */
+ void rotatePoints(const std::vector<Point2d>& pointsIn, double angle, std::vector<Point2d>& pointsOut);
+
+ /** @brief Computes the corner score, position and orientation of point with given index. */
+ double computeCornerScore(const std::vector<Point2d>& scan, int index, OC& keypoint);
+
+ /** @brief Finds the neighborhood of point with the given index in vector.
+ * @param pointsIn vector of all the points
+ * @param index index of the center point in the
+ * @param neigh found neighborhood
+ * @param midIndex index of the center point in neigh vector
+ */
+ void getNeighPoints(const std::vector<Point2d>& pointsIn, int index, std::vector<Point2d>& neigh, int& midIndex, double& dist) const;
+
+ /** @brief Finds the peaks of score function using Non-Maxima Suppression (NMS) over a circular neighborhood with given radius.
+ * It is almost a duplicate of FALKOExtractor similar function.
+ */
+ void NMSKeypoint(const std::vector<double>& scores, const LaserScan& scan, unsigned int ibeg, unsigned int iend, double radius, int minval, std::vector<int>& peaks);
+ };
+}
diff --git a/include/falkolib/Matching/AHTMatcher.h b/include/falkolib/Matching/AHTMatcher.h
new file mode 100644
index 0000000000000000000000000000000000000000..8dcd985fba4fdfd2e77ef088ab69b00be2f01435
--- /dev/null
+++ b/include/falkolib/Matching/AHTMatcher.h
@@ -0,0 +1,182 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <falkolib/Matching/Matcher.h>
+
+#include <vector>
+#include <falkolib/Feature/Descriptor.h>
+#include <falkolib/Feature/Keypoint.h>
+
+namespace falkolib {
+
+ /**
+ * @brief class representing a Affine Hough Transform feature matching engine
+ */
+ template <typename T = Keypoint, typename D = Descriptor>
+ class AHTMatcher : public Matcher<T> {
+ public:
+
+ /**
+ * @brief Constructor
+ */
+ AHTMatcher() : AHTMatcher(0.1, 0.1, 0.04, 5, 5, 1.57) {
+ };
+
+ /**
+ * @brief Constructor
+ */
+ AHTMatcher(double _xRes, double _yRes, double _thetaRes, double _xAbsMax, double _yAbsMax, double _thetaAbsMax) {
+ xRes = _xRes;
+ yRes = _yRes;
+ thetaRes = _thetaRes;
+ xAbsMax = _xAbsMax;
+ yAbsMax = _yAbsMax;
+ thetaAbsMax = _thetaAbsMax;
+ xSize = static_cast<int> (2.0 * xAbsMax / xRes);
+ ySize = static_cast<int> (2.0 * yAbsMax / yRes);
+ thetaSize = static_cast<int> (2.0 * thetaAbsMax / thetaRes);
+ matchesGrid.resize(xSize * ySize * thetaSize);
+ txMax = 0;
+ tyMax = 0;
+ thetaMax = 0;
+ };
+
+ /**
+ * @brief match keypoints between two sets using AHT matcher
+ * @param v1 first set of keypoints
+ * @param v2 second set of keypoints
+ * @param match matching vector representing associations, pair.first corresponds to v1 and pair.second corresponds to v2
+ * @return number of valid association in match
+ */
+ int match(const std::vector<T>& v1, const std::vector<T>& v2, std::vector<std::pair<int, int> >& match) {
+ std::vector<std::pair<int, int> > assoInit;
+ for (int i1 = 0; i1 < v1.size(); ++i1) {
+ for (int i2 = 0; i2 < v2.size(); ++i2) {
+ if (v1[i1].distance(v2[i2]) < distTh) {
+ assoInit.push_back(std::make_pair(i1, i2));
+ }
+ }
+ }
+
+ return getBestMatching(v1, v2, assoInit, match);
+ }
+
+ /**
+ * @brief match keypoints between two sets using descriptors
+ * @param keypoints1 first set of keypoints
+ * @param descriptors1 first set of descriptors
+ * @param keypoints2 second set of keypoints
+ * @param descriptors2 second set of descriptors
+ * @param match matching vector representing associations, pair.first corresponds to v1 and pair.second corresponds to v2
+ * @return number of valid association in match
+ */
+ int match(const std::vector<T>& keypoints1, const std::vector<D>& descriptors1, const std::vector<T>& keypoints2, const std::vector<D>& descriptors2, std::vector<std::pair<int, int> >& match) {
+ std::vector<std::pair<int, int> > assoInit;
+ for (int i1 = 0; i1 < keypoints1.size(); ++i1) {
+ for (int i2 = 0; i2 < keypoints2.size(); ++i2) {
+ if (keypoints1[i1].distance(keypoints2[i2]) < distTh && descriptors1[i1].distance(descriptors2[i2]) < descTh) {
+ assoInit.push_back(std::make_pair(i1, i2));
+ }
+ }
+ }
+
+ return getBestMatching(keypoints1, keypoints2, assoInit, match);
+ }
+
+ /** @brief set euclidean distance threshold for keypoints distance measurement*/
+ void setDistanceThreshold(double _th) {
+ distTh = _th;
+ }
+
+ /** @brief set descriptor threshold for distance measurement*/
+ void setDescriptorThreshold(double _th) {
+ descTh = _th;
+ }
+
+
+
+ private:
+ double distTh;
+ double descTh;
+ double xRes;
+ double yRes;
+ double thetaRes;
+ double xAbsMax;
+ double yAbsMax;
+ double thetaAbsMax;
+ int xSize;
+ int ySize;
+ int thetaSize;
+ std::vector<std::vector<std::pair<int, int> > > matchesGrid;
+ // std::vector<int> countGrid;
+ int txMax;
+ int tyMax;
+ int thetaMax;
+
+ /** @brief get grid index from separated transform indexes*/
+ int getGridIndex(int ix, int iy, int it) {
+ return ix + iy * xSize + it * xSize * ySize;
+ }
+
+ /** @brief get best matching given point and an initial guest associations list*/
+ int getBestMatching(const std::vector<T>& v1, const std::vector<T>& v2, const std::vector<std::pair<int, int> >& init, std::vector<std::pair<int, int> >& match) {
+ for (auto& asso : init) {
+ for (int it = 0; it < thetaSize; ++it) {
+ double theta = thetaRes * (it - thetaSize / 2);
+ const Point2d& p1 = v1[asso.first].point;
+ const Point2d& p2 = v2[asso.second].point;
+ double tx = p1[0] - p2[0] * std::cos(theta) + p2[1] * std::sin(theta);
+ double ty = p1[1] - p2[0] * std::sin(theta) - p2[1] * std::cos(theta);
+
+ int ix = static_cast<int> (tx / xRes + xSize / 2);
+ int iy = static_cast<int> (ty / yRes + ySize / 2);
+
+ if (ix >= 0 && ix < xSize && iy >= 0 && iy < ySize && it >= 0 && it < thetaSize) {
+// std::cout << "ind: " << getGridIndex(ix, iy, it) << "\t" << ix << "\t" << iy << "\t" << it << "\tpoints: " << p1.transpose() << "\t" << p2.transpose() << std::endl;
+ matchesGrid[getGridIndex(ix, iy, it)].push_back(asso);
+
+ if (matchesGrid[getGridIndex(ix, iy, it)].size() >= matchesGrid[getGridIndex(txMax, tyMax, thetaMax)].size()) {
+ txMax = ix;
+ tyMax = iy;
+ thetaMax = it;
+ }
+ }
+ }
+ }
+
+ match = matchesGrid[getGridIndex(txMax, tyMax, thetaMax)];
+ int numAsso = match.size();
+ for (int i = 0; i < v1.size(); ++i) {
+ bool found = false;
+ for (int j = 0; j < match.size(); ++j) {
+ if (i == match[j].first) {
+ found = true;
+ break;
+ }
+ }
+ if (not found) {
+ match.push_back(std::make_pair(i, -1));
+ }
+ }
+ return numAsso;
+ }
+ };
+}
\ No newline at end of file
diff --git a/include/falkolib/Matching/CCDAMatcher.h b/include/falkolib/Matching/CCDAMatcher.h
new file mode 100644
index 0000000000000000000000000000000000000000..f94abc51b90bb39b4cde4120763e6e235e122286
--- /dev/null
+++ b/include/falkolib/Matching/CCDAMatcher.h
@@ -0,0 +1,235 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <Eigen/Dense>
+#include <iostream>
+#include <falkolib/Matching/Matcher.h>
+#include <mcqd/mcqd.h>
+
+namespace falkolib {
+
+ /** @brief This class implements the Combined Constraint Data Association (CCDA)
+ * proposed by Tim Bailey. The best reference is:
+ *
+ * T. Bailey, "Mobile Robot Localisation and Mapping in Extensive Outdoor Environments",
+ * PhD Thesis, University of Sydney, 2002.
+ *
+ * Of course, there are the related papers.
+ * The method is based on the so called CorrespondenceGraph built on two point sets.
+ * Data Association is computed as the maximum clique CorrespondenceGraph.
+ *
+ * Maximum clique has been computed according to T. Bailey suggestions.
+ * However, for different approaches see:
+ * http://www.dharwadker.org/clique/
+ * http://www.sanfoundry.com/cpp-program-find-maximum-size-clique-graph/
+ * http://www.sicmm.org/~konc/maxclique/mcqd/mcqd.h
+ */
+ template <typename T = Keypoint, typename D = Descriptor>
+ class CCDAMatcher : public Matcher<T> {
+ public:
+
+ /** Node of correspondence graph represents an association
+ * between an input point and a target point.
+ */
+ struct Node {
+ int index; // global index (that should correspond to position in nodes vector)
+ int inputId; // input point ID
+ int targetId; // target point ID
+ std::vector<int> adjacents;
+
+ bool operator<(const Node& n) const {
+ return (index < n.index);
+ }
+
+ int degree() const {
+ return adjacents.size();
+ }
+ };
+
+ /** Distance constraint between the point belonging to the same set.
+ */
+ struct Constraint {
+ int i;
+ int j;
+ double dist;
+
+ bool operator<(const Constraint& c) const {
+ return (dist < c.dist);
+ }
+ };
+
+ /** @brief Default constructor. */
+ CCDAMatcher() : distTol(0.10), distMin(0.0) {
+ }
+
+ /** @brief Sets the tolerance on distances differences to be compatible. See CCDA details. */
+ void setDistTol(double _distTol) {
+ distTol = _distTol;
+ }
+
+ /** @brief Sets the minimum distance between two keypoints to be a constraint. */
+ void setDistMin(double _distMin) {
+ distMin = _distMin;
+ }
+
+ /** Computes the value
+ */
+ int match(const std::vector<T>& v1, const std::vector<T>& v2, std::vector<std::pair<int, int> >& match) {
+ std::vector<Node> nodes;
+ std::vector<std::pair<int, int> > edges;
+ std::vector<Constraint> constraints1;
+ std::vector<Constraint> constraints2;
+ Constraint constrTmp;
+ std::vector<int> cliqueMax;
+ int num1 = v1.size();
+ int num2 = v2.size();
+ int isrc, idst;
+
+ match.clear();
+ // Creates constraints inside each group of keypoints.
+ // A constraint is the geometric distance (must be > distMin!) between two internal keypoints of a given set.
+ makeRelativeConstraints(v1, constraints1);
+ makeRelativeConstraints(v2, constraints2);
+
+ // Creates correspondence graph based on constraints:
+ // * node: association hypothesis n=(k1,k2) where k1 in set1, k2 in set2;
+ // * edge: connects 2 compatible association hypotheses (compatibility (na,nb) if ||na.k1-na.k2| - |nb.k1-nb.k2|| < distToll);
+
+ // Nodes
+ makeNodeSet(v1, v2, nodes);
+
+ // Edges
+ for (auto& constrCurr : constraints1) {
+ // Finds the candidate target constraints, i.e. pair of target points with similar
+ // distance, and visits them
+ constrTmp.dist = constrCurr.dist - distTol;
+ typename std::vector<Constraint>::iterator it = std::upper_bound(constraints2.begin(), constraints2.end(), constrTmp);
+ for (; it != constraints2.end() && it->dist < constrCurr.dist + distTol; ++it) {
+ //std::cout << " target constr (" << it->i << "," << it->j << "): " << it->dist << std::endl;
+ if (std::abs(it->dist - constrCurr.dist) < distTol && (it->dist + constrCurr.dist) > distMin) {
+ assert(constrCurr.i < num1 && constrCurr.j < num1);
+ assert(it->i < num2 && it->j < num2);
+ // Match 1
+ isrc = it->i + constrCurr.i * num2;
+ idst = it->j + constrCurr.j * num2;
+ assert(isrc < nodes.size() && idst < nodes.size());
+ assert(nodes[isrc].inputId == constrCurr.i && nodes[isrc].targetId == it->i);
+ assert(nodes[idst].inputId == constrCurr.j && nodes[idst].targetId == it->j);
+ nodes[isrc].adjacents.push_back(idst);
+ nodes[idst].adjacents.push_back(isrc);
+ // Match 2
+ isrc = it->i + constrCurr.j * num2;
+ idst = it->j + constrCurr.i * num2;
+ assert(isrc < nodes.size() && idst < nodes.size());
+ assert(nodes[isrc].inputId == constrCurr.j && nodes[isrc].targetId == it->i);
+ assert(nodes[idst].inputId == constrCurr.i && nodes[idst].targetId == it->j);
+ nodes[isrc].adjacents.push_back(idst);
+ nodes[idst].adjacents.push_back(isrc);
+ }
+ }
+ }
+
+ // Finds maximum clique
+ findCliqueDyn(nodes, cliqueMax);
+ match.reserve(cliqueMax.size());
+ for (auto& id : cliqueMax) {
+ match.push_back(std::make_pair(nodes[id].inputId, nodes[id].targetId));
+ }
+ }
+
+ private:
+ double distTol;
+ double distMin;
+
+ void makeNodeSet(const std::vector<T>& points1, const std::vector<T>& points2, std::vector<Node>& nodes) {
+ Node corresp;
+ int index;
+ nodes.clear();
+ nodes.resize(points1.size() * points2.size());
+ for (int i = 0; i < points1.size(); ++i) {
+ for (int j = 0; j < points2.size(); ++j) {
+ index = i * points2.size() + j;
+ nodes[index].inputId = i;
+ nodes[index].targetId = j;
+ nodes[index].index = index;
+ nodes[index].adjacents.clear();
+ }
+ }
+ // for (int i = 0; i < nodes.size(); ++i) {
+ // if (nodes[i].index != i) {
+ // std::cerr << __PRETTY_FUNCTION__ << ": difference bewteen node index " << nodes[i].index << " and its position " << i << " in node vector" << std::endl;
+ // }
+ // }
+ }
+
+ void makeRelativeConstraints(const std::vector<T>& points, std::vector<Constraint>& constraints) {
+ Constraint constraint;
+ int n = points.size();
+ for (int i = 0; i < n; ++i) {
+ for (int j = i + 1; j < n; ++j) {
+ constraint.i = i;
+ constraint.j = j;
+ constraint.dist = points[i].distance(points[j]);
+ constraints.push_back(constraint);
+ }
+ }
+ std::sort(constraints.begin(), constraints.end());
+ }
+
+ void findCliqueDyn(const std::vector<Node>& nodes, std::vector<int>& cliqueMax) {
+ bool **conn;
+ int nsize = nodes.size();
+ int *qmax;
+ int qsize;
+
+ if (nsize == 0) {
+ return;
+ }
+
+ // Allocates space for adjacence matrix
+ conn = new bool*[nsize];
+ for (int i = 0; i < nsize; ++i) {
+ conn[i] = new bool[nsize];
+ memset(conn[i], 0, nsize * sizeof (bool));
+ }
+ // Fills adjacence matrix
+ for (int i = 0; i < nodes.size(); ++i) {
+ assert(nodes[i].index == i);
+ for (auto& j : nodes[i].adjacents) {
+ conn[i][j] = true;
+ }
+ }
+ // Computes maximum clique
+ Maxclique maxcl(conn, nsize);
+ maxcl.mcq(qmax, qsize);
+ cliqueMax.resize(qsize);
+ for (int i = 0; i < qsize; ++i) {
+ cliqueMax[i] = qmax[i];
+ }
+
+ delete [] qmax;
+ for (int i = 0; i < nsize; ++i)
+ delete [] conn[i];
+ delete [] conn;
+ }
+
+ };
+}
diff --git a/include/falkolib/Matching/Matcher.h b/include/falkolib/Matching/Matcher.h
new file mode 100644
index 0000000000000000000000000000000000000000..cace20b196fc4201b66d42da2c6a3064a4327cea
--- /dev/null
+++ b/include/falkolib/Matching/Matcher.h
@@ -0,0 +1,42 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <vector>
+
+namespace falkolib {
+
+ /**
+ * @brief class representing a simple feature matching engine
+ */
+ template <typename T>
+ class Matcher {
+ protected:
+
+ /**
+ * @brief match features between two sets
+ * @param v1 first set of features
+ * @param v2 second set of features
+ * @param match matching vector representing associations, pair.first corresponds to v1 and pair.second corresponds to v2
+ * @return number of valid association in match
+ */
+ virtual int match(const std::vector<T>& v1, const std::vector<T>& v2, std::vector<std::pair<int, int> >& match) = 0;
+ };
+}
\ No newline at end of file
diff --git a/include/falkolib/Matching/NNMatcher.h b/include/falkolib/Matching/NNMatcher.h
new file mode 100644
index 0000000000000000000000000000000000000000..acda421e2cb95c6b536a43afdbf16a504aa08752
--- /dev/null
+++ b/include/falkolib/Matching/NNMatcher.h
@@ -0,0 +1,136 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <falkolib/Matching/Matcher.h>
+
+#include <vector>
+#include <falkolib/Feature/Descriptor.h>
+#include <falkolib/Feature/Keypoint.h>
+
+namespace falkolib {
+
+ /**
+ * @brief class representing a simple Nearest-Neighborhood feature matching engine
+ */
+ template <typename T = Keypoint, typename D = Descriptor>
+ class NNMatcher : public Matcher<T> {
+ public:
+
+ /**
+ * @brief Constructor
+ */
+ NNMatcher() {
+
+ };
+
+ /**
+ * @brief match keypoints or descriptors between two sets
+ * @param v1 first set of keypoints or descriptors
+ * @param v2 second set of keypoints or descriptors
+ * @param match matching vector representing associations, pair.first corresponds to v1 and pair.second corresponds to v2
+ * @return number of valid association in match
+ */
+ int match(const std::vector<T>& v1, const std::vector<T>& v2, std::vector<std::pair<int, int> >& match) {
+ match.clear();
+ int imin;
+ double dmin, d;
+ int counter = 0;
+ std::vector<bool> matched(v2.size(), false);
+ for (int i1 = 0; i1 < (int) v1.size(); ++i1) {
+ imin = -1;
+ dmin = 1.05 * distTh;
+ for (int i2 = 0; i2 < (int) v2.size(); ++i2) {
+ if (not matched[i2]) {
+ d = v1[i1].distance(v2[i2]);
+ if (d < dmin) {
+ imin = i2;
+ dmin = d;
+ }
+ }
+ }
+ if (dmin < distTh) {
+ match.push_back(std::make_pair(i1, imin));
+ matched[imin] = true;
+ counter++;
+ } else {
+ match.push_back(std::make_pair(i1, -1));
+ }
+ }
+
+ return counter;
+ }
+
+ /**
+ * @brief match keypoints between two sets using descriptors
+ * @param keypoints1 first set of keypoints
+ * @param descriptors1 first set of descriptors
+ * @param keypoints2 second set of keypoints
+ * @param descriptors2 second set of descriptors
+ * @param match matching vector representing associations, pair.first corresponds to v1 and pair.second corresponds to v2
+ * @return number of valid association in match
+ */
+ int match(const std::vector<T>& keypoints1, const std::vector<D>& descriptors1, const std::vector<T>& keypoints2, const std::vector<D>& descriptors2, std::vector<std::pair<int, int> >& match) {
+ match.clear();
+ int imin;
+ double dmin, d, ddesc;
+ int counter = 0;
+ std::vector<bool> matched(keypoints2.size(), false);
+ for (int i1 = 0; i1 < (int) keypoints1.size(); ++i1) {
+ imin = -1;
+ dmin = 1.05 * distTh;
+ for (int i2 = 0; i2 < (int) keypoints2.size(); ++i2) {
+ if (not matched[i2]) {
+ d = keypoints1[i1].distance(keypoints2[i2]);
+ ddesc = descriptors1[i1].distance(descriptors2[i2]);
+ if (d < dmin && ddesc < descTh) {
+ imin = i2;
+ dmin = d;
+ }
+ }
+ }
+ if (dmin < distTh && descriptors1[i1].distance(descriptors2[imin]) < descTh) {
+ match.push_back(std::make_pair(i1, imin));
+ matched[imin] = true;
+ counter++;
+ } else {
+ match.push_back(std::make_pair(i1, -1));
+ }
+ }
+
+ return counter;
+ }
+
+ /** @brief set euclidean distance threshold for keypoints distance measurements*/
+ void setDistanceThreshold(double _th) {
+ distTh = _th;
+ }
+
+ /** #brief set descriptor threshold for distance measurements*/
+ void setDescriptorThreshold(double _th) {
+ descTh = _th;
+ }
+
+ private:
+ double distTh;
+ double descTh;
+
+ };
+}
\ No newline at end of file
diff --git a/src/Common/HoughSpectrum.cpp b/src/Common/HoughSpectrum.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cb2a1479144ccde986bfc63180d3d8e78299187a
--- /dev/null
+++ b/src/Common/HoughSpectrum.cpp
@@ -0,0 +1,74 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <falkolib/Common/HoughSpectrum.h>
+#include <cassert>
+
+using namespace std;
+
+namespace falkolib {
+
+ HoughSpectrum::HoughSpectrum()
+ : thetaNum_(360),
+ rhoNum_(1000),
+ thetaStep_(0.0087266), rhoStep_(0.02),
+ hough_(thetaNum_, rhoNum_),
+ spectrum_(thetaNum_),
+ orthoSpectrum_((thetaNum_ / 2) + (thetaNum_ % 2)),
+ cosLut_(thetaNum_), sinLut_(thetaNum_) {
+ for (unsigned int i = 0; i < thetaNum_; ++i) {
+ cosLut_(i) = cos(thetaStep_ * i);
+ sinLut_(i) = sin(thetaStep_ * i);
+ }
+ }
+
+ HoughSpectrum::HoughSpectrum(double thetaStep, double rhoStep, double rhoMax)
+ : thetaNum_(static_cast<unsigned int> (ceil(M_PI / thetaStep))),
+ rhoNum_(2 * static_cast<unsigned int> (ceil(rhoMax / rhoStep))),
+ thetaStep_(thetaStep), rhoStep_(rhoStep),
+ hough_(thetaNum_, rhoNum_),
+ spectrum_(thetaNum_),
+ orthoSpectrum_((thetaNum_ / 2) + (thetaNum_ % 2)),
+ cosLut_(thetaNum_), sinLut_(thetaNum_) {
+ for (unsigned int i = 0; i < thetaNum_; ++i) {
+ cosLut_(i) = cos(thetaStep_ * i);
+ sinLut_(i) = sin(thetaStep_ * i);
+ }
+ }
+
+ void HoughSpectrum::init(double thetaStep, double rhoStep, double rhoMax) {
+ thetaStep_ = thetaStep;
+ rhoStep_ = rhoStep;
+ thetaNum_ = (unsigned int) ceil(M_PI / thetaStep);
+ rhoNum_ = (unsigned int) ceil(rhoMax / rhoStep);
+
+ hough_.resize(thetaNum_, rhoNum_);
+ spectrum_.resize(thetaNum_);
+ orthoSpectrum_.resize(thetaNum_ / 2);
+ cosLut_.resize(thetaNum_);
+ sinLut_.resize(thetaNum_);
+
+ for (unsigned int i = 0; i < thetaNum_; ++i) {
+ cosLut_(i) = cos(thetaStep_ * i);
+ sinLut_(i) = sin(thetaStep_ * i);
+ }
+ }
+
+} // end of namespace
+
diff --git a/src/Feature/BSC.cpp b/src/Feature/BSC.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..98f73d8546f759b8073d32f66951da03b0074ba1
--- /dev/null
+++ b/src/Feature/BSC.cpp
@@ -0,0 +1,85 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <falkolib/Feature/BSC.h>
+#include <falkolib/Common/GeomUtils.h>
+#include <limits>
+#include <math.h>
+#include <assert.h>
+#include <algorithm>
+
+using namespace std;
+
+namespace falkolib {
+
+ BSC::BSC(double _radius, int _circularSectorNumber, int _radialRingNumber) {
+ radius = _radius;
+ circularSectorNumber = _circularSectorNumber;
+ sectorResolution = 2.0 * M_PI / circularSectorNumber;
+ radialRingNumber = _radialRingNumber;
+ ringResolution = radius / radialRingNumber;
+ }
+
+ void BSC::compute(std::vector<Point2d>& neigh, int centralPointIndex) {
+ const int size = neigh.size();
+ grid.resize(radialRingNumber, std::vector<uint8_t>(circularSectorNumber, 0));
+ for (int i = 0; i < size; ++i) {
+ if (i != centralPointIndex) {
+ int col = static_cast<int> (floor((angleBetweenPoints(neigh[i], neigh[centralPointIndex]) + M_PI) / sectorResolution));
+ assert(col < circularSectorNumber);
+ int row = static_cast<int> (floor(((neigh[i] - neigh[centralPointIndex]).norm()) / ringResolution));
+ assert(row < radialRingNumber);
+ grid[row][col] = 1;
+ }
+ }
+ }
+
+ double BSC::distance(const Descriptor& desc) const{
+ try {
+ const BSC& d2 = dynamic_cast<const BSC&> (desc);
+ assert(circularSectorNumber == d2.circularSectorNumber);
+ assert(radialRingNumber == d2.radialRingNumber);
+ if (circularSectorNumber == d2.circularSectorNumber && radialRingNumber == d2.radialRingNumber) {
+ return HammingDistance(grid, d2.grid);
+ }
+ } catch (const std::bad_cast& e){
+ ;
+ }
+ return numeric_limits<double>::max();
+ }
+
+ double BSC::HammingDistance(const std::vector<std::vector<uint8_t> >& g1, const std::vector<std::vector<uint8_t> >& g2) const {
+ double diff = 0.0;
+ for (int i = 0; i < g1.size(); ++i) {
+ for (int j = 0; j < g1[i].size(); ++j) {
+ if (g1[i][j] != g2[i][j]) {
+ diff++;
+ }
+ }
+ }
+ return diff;
+ }
+
+ void BSC::rotate(double theta) {
+ int rot = static_cast<int> (floor((theta + M_PI) / sectorResolution)) % circularSectorNumber;
+ for (auto& i : grid) {
+ std::rotate(i.begin(), i.begin() + rot, i.end());
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/Feature/CGH.cpp b/src/Feature/CGH.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e551bb1f09df522a9fa9d137c7bc75e24a682c8d
--- /dev/null
+++ b/src/Feature/CGH.cpp
@@ -0,0 +1,93 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <falkolib/Feature/CGH.h>
+#include <falkolib/Common/GeomUtils.h>
+#include <limits>
+#include <math.h>
+#include <assert.h>
+#include <algorithm>
+
+#include <iostream>
+
+using namespace std;
+
+namespace falkolib {
+
+ CGH::CGH(double _radius, int _circularSectorNumber) {
+ radius = _radius;
+ circularSectorNumber = _circularSectorNumber;
+ sectorResolution = 2.0 * M_PI / circularSectorNumber;
+ }
+
+ void CGH::compute(std::vector<Point2d>& neigh, int centralPointIndex) {
+ const int size = neigh.size();
+ histogram.resize(circularSectorNumber, 0);
+ for (int i = 0; i < size; ++i) {
+ if (i != centralPointIndex) {
+ int ind = static_cast<int> (floor((angleBetweenPoints(neigh[i], neigh[centralPointIndex]) + M_PI) / sectorResolution));
+ assert(ind < circularSectorNumber);
+ if (ind == 0) {
+ histogram[circularSectorNumber - 1]++;
+ } else {
+ histogram[ind - 1]++;
+ }
+ histogram[ind] += 5;
+ if (ind == (circularSectorNumber - 1)) {
+ histogram[0]++;
+ } else {
+ histogram[ind + 1]++;
+ }
+ }
+ }
+ double sum = 0;
+ for (auto& l : histogram) {
+ sum += l;
+ }
+ for (auto& l : histogram) {
+ l /= sum;
+ }
+ }
+
+ double CGH::distance(const Descriptor& desc) const {
+ try {
+ const CGH& d2 = dynamic_cast<const CGH&> (desc);
+ assert(circularSectorNumber == d2.circularSectorNumber);
+ if (circularSectorNumber == d2.circularSectorNumber) {
+ return SymmetricChiSquaredDistance(histogram, d2.histogram);
+ }
+ } catch (const std::bad_cast& e){
+ ;
+ }
+ return numeric_limits<double>::max();
+ }
+
+ double CGH::SymmetricChiSquaredDistance(const std::vector<double>& h1, const std::vector<double>& h2) const {
+ double sum1 = 0.0;
+ for (int i = 0; i < h1.size(); ++i) {
+ sum1 += ((h1[i] - h2[i])*(h1[i] - h2[i]) / (h1[i] + h2[i] + 0.00000001));
+ }
+ return sum1/2.0;
+ }
+
+ void CGH::rotate(double theta) {
+ int rot = (static_cast<int> (floor((theta + M_PI) / sectorResolution)) % circularSectorNumber);
+ std::rotate(histogram.begin(), histogram.begin()+rot, histogram.end());
+ }
+}
\ No newline at end of file
diff --git a/src/Feature/FALKOExtractor.cpp b/src/Feature/FALKOExtractor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9abe9417ba857325cdb05e9eff266fbda0222320
--- /dev/null
+++ b/src/Feature/FALKOExtractor.cpp
@@ -0,0 +1,331 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <falkolib/Feature/FALKOExtractor.h>
+
+using namespace std;
+
+namespace falkolib {
+
+ FALKOExtractor::FALKOExtractor() {
+ minScoreTh = 50;
+ minExtractionRange = 0;
+ maxExtractionRange = 30;
+ subbeam = true;
+ NMSRadius = 0.1;
+ neighA = 0.1;
+ neighB = 0.07;
+ neighMinPoint = 2;
+ bRatio = 2.5;
+ gridSectors = 16;
+ }
+
+ void FALKOExtractor::extract(const LaserScan& scan, std::vector<FALKO>& keypoints) {
+ const int numBeams = scan.getNumBeams();
+ vector<int> scores(numBeams, -10);
+ vector<Point2d> neigh;
+ vector<double> radius(numBeams);
+ int neighSize;
+ int neighSizeL;
+ int neighSizeR;
+ int midIndex;
+ double triangleBLength;
+ double triangleHLength;
+ vector<double> thetaCorner(numBeams);
+ int scoreL;
+ int scoreR;
+ int scoreMax = 0;
+ vector<int> peaks;
+ vector<int> neighCircularIndexesL;
+ vector<int> neighCircularIndexesR;
+
+ for (int ind = 0; ind < numBeams; ++ind) {
+ if (scan.ranges[ind] < minExtractionRange || scan.ranges[ind] > maxExtractionRange) {
+ scores[ind] = -10;
+ continue;
+ }
+ neigh.clear();
+ radius[ind] = getNeighRadius(scan.ranges[ind]);
+ scan.getNeighPoints(ind, radius[ind], neigh, midIndex);
+ neighSize = neigh.size();
+
+ neighSizeL = midIndex;
+ neighSizeR = neighSize - midIndex - 1;
+
+ if (neighSizeL < neighMinPoint || neighSizeR < neighMinPoint) {
+ scores[ind] = -10;
+ continue;
+ }
+
+ triangleBLength = pointsDistance(neigh.front(), neigh.back());
+ triangleHLength = std::abs(signedTriangleArea(neigh[midIndex], neigh.front(), neigh.back())) / triangleBLength;
+
+ if (triangleBLength < (radius[ind] / bRatio) || triangleHLength < (radius[ind] / bRatio)) {
+ scores[ind] = -10;
+ continue;
+ }
+
+ thetaCorner[ind] = getCornerOrientation(neigh, midIndex);
+
+ neighCircularIndexesL.resize(neighSizeL, 0);
+ neighCircularIndexesR.resize(neighSizeR, 0);
+
+ for (int i = 0; i < neighSizeL; ++i) {
+ neighCircularIndexesL[i] = getCircularSectorIndex(neigh[i], neigh[midIndex], thetaCorner[ind]);
+ }
+ for (int i = 0; i < neighSizeR; ++i) {
+ neighCircularIndexesR[i] = getCircularSectorIndex(neigh[midIndex + i + 1], neigh[midIndex], thetaCorner[ind]);
+ }
+
+ scoreL = 0;
+ scoreR = 0;
+
+ for (int i = midIndex - 1; i >= 0; --i) {
+ for (int j = i; j >= 0; --j) {
+ scoreL += circularSectorDistance(neighCircularIndexesL[i], neighCircularIndexesL[j], gridSectors);
+ }
+ }
+
+ for (int i = midIndex + 1; i < neighSize; ++i) {
+ for (int j = i; j < neighSize; ++j) {
+ scoreR += circularSectorDistance(neighCircularIndexesR[i - midIndex - 1], neighCircularIndexesR[j - midIndex - 1], gridSectors);
+ }
+ }
+
+ scores[ind] = scoreL + scoreR;
+
+ if (scores[ind] > scoreMax) {
+ scoreMax = scores[ind];
+ }
+ }
+
+ for (int ind = 0; ind < numBeams; ++ind) {
+ if (scores[ind] < 0) {
+ scores[ind] = scoreMax;
+ }
+ scores[ind] = scoreMax - scores[ind];
+ }
+
+ NMSKeypoint(scores, scan, 0, numBeams, NMSRadius, (scoreMax * minScoreTh / 100.0), peaks);
+
+ for (int i = 0; i < peaks.size(); ++i) {
+ FALKO kp;
+ kp.index = peaks[i];
+ kp.orientation = thetaCorner[peaks[i]];
+ kp.radius = radius[peaks[i]];
+
+ if (subbeam) {
+ subBeamCorner(scan, peaks[i], radius[peaks[i]], kp.point);
+ } else {
+ kp.point = scan.points[peaks[i]];
+ }
+
+ keypoints.push_back(std::move(kp));
+ }
+
+ }
+
+ int FALKOExtractor::circularSectorDistance(int a1, int a2, int res) {
+ const int r2 = res / 2;
+ return std::abs(((a1 - a2) + r2) % res - r2);
+ }
+
+ int FALKOExtractor::getCircularSectorIndex(const Point2d& p, const Point2d& pmid, double theta) {
+ return (int) floor((angleBetweenPoints(p, pmid) + M_PI - theta) / (2.0 * M_PI / gridSectors));
+ }
+
+ double FALKOExtractor::getNeighRadius(double rho) {
+ double radius = neighA * std::exp(neighB * rho);
+ if (radius >= rho) {
+ return rho * 0.8;
+ }
+ return radius;
+ }
+
+ double FALKOExtractor::getCornerOrientation(const std::vector<Point2d>& neigh, int midIndex) {
+ Point2d oriL(0, 0);
+ Point2d oriR(0, 0);
+ int size = neigh.size();
+ for (int i = 0; i < size; ++i) {
+ if (i < midIndex) {
+ oriL += (neigh[i] - neigh[midIndex]); // / (s[i] - s[mid]).norm();
+ } else if (i > midIndex) {
+ oriR += (neigh[i] - neigh[midIndex]); // / (s[i] - s[mid]).norm();
+ }
+ }
+ oriL /= midIndex;
+ oriR /= (size - (midIndex + 1));
+ Point2d ori = oriL + oriR;
+ double theta = atan2(ori(1), ori(0));
+ }
+
+ void FALKOExtractor::NMSKeypoint(const std::vector<int>& scores, const LaserScan& scan, unsigned int ibeg, unsigned int iend, double radius, int minval, std::vector<int>& peaks) {
+ unsigned i, imax;
+ unsigned j, jbeg, jend, jmax;
+ std::vector<int> candidates;
+ peaks.clear();
+
+ i = ibeg;
+ imax = ibeg;
+ while (i < iend) {
+ int win;
+ if (radius >= scan.ranges[i]) {
+ win = std::floor(std::asin(0.8) / scan.getAngleInc());
+ } else {
+ win = std::floor(std::asin(radius / scan.ranges[i]) / scan.getAngleInc());
+ }
+
+ jmax = i;
+
+ if (imax + win < i) {
+ jbeg = (i >= win ? i - win : 0);
+ imax = i;
+ } else {
+ jbeg = i;
+ }
+
+ jend = (i + win + 1 < iend ? i + win + 1 : iend);
+ for (j = jbeg; j < jend; ++j) {
+ if (scores[j] > scores[jmax]) {
+ jmax = j;
+ }
+ }
+ imax = (scores[jmax] >= scores[imax] ? jmax : imax);
+
+ if (i == imax && scores[i] > minval) {
+ candidates.push_back(i);
+ }
+ if (jmax > i) i = jmax;
+ else ++i;
+ }
+
+ int i1 = 0;
+ int i2 = 0;
+ int counter = 0;
+
+ for (i1 = 0, i2 = 0; i1 < candidates.size(); ++i1) {
+ if (scores[candidates[i2]] == scores[candidates[i1]]) {
+ counter++;
+ if (2 * abs(i2 - i1) > counter) {
+ ++i2;
+ }
+ } else {
+ peaks.push_back(candidates[i2]);
+ i2 = i1;
+ counter = 0;
+ }
+ }
+ if (i2 != candidates.size()) {
+ peaks.push_back(candidates[i2]);
+ }
+ }
+
+ void FALKOExtractor::subBeamCorner(const LaserScan& scan, int index, double radius, Point2d& p) {
+ vector<Point2d> neigh;
+ int midIndex;
+ scan.getNeighPoints(index, radius, neigh, midIndex);
+ int neighSize = neigh.size();
+
+ Eigen::Vector3d leftLine;
+ Eigen::Vector3d rightLine;
+
+ std::vector<Point2d> leftSide;
+ for (int i = 0; i <= midIndex; ++i) {
+ leftSide.push_back(neigh[i]);
+ }
+
+ std::vector<Point2d> rightSide;
+ for (int i = midIndex; i < neighSize; ++i) {
+ rightSide.push_back(neigh[i]);
+ }
+
+ generateLine(leftSide, leftLine);
+ generateLine(rightSide, rightLine);
+
+ double A[4];
+ double b[2];
+ double x[2];
+ A[0] = leftLine[0];
+ A[1] = leftLine[1];
+ A[2] = rightLine[0];
+ A[3] = rightLine[1];
+
+ b[0] = -leftLine[2];
+ b[1] = -rightLine[2];
+
+ bool valid = solveSystem2x2(A, b, x);
+
+ if (not valid) {
+ p = neigh[midIndex];
+ return;
+ }
+
+ Point2d temp(x[0], x[1]);
+
+ if (pointsDistance(neigh[midIndex], temp) < 0.20) {
+ p = temp;
+ } else {
+ p = neigh[midIndex];
+ }
+ }
+
+ void FALKOExtractor::generateLine(const std::vector<Point2d>& points, Eigen::Vector3d& model) {
+ double sxx = 0.0;
+ double syy = 0.0;
+ double sxy = 0.0;
+ double sx = 0.0;
+ double sy = 0.0;
+ int num = 0;
+ for (unsigned int i = 0; i < points.size(); ++i) {
+ sxx += points[i](0) * points[i](0);
+ syy += points[i](1) * points[i](1);
+ sxy += points[i](0) * points[i](1);
+ sx += points[i](0);
+ sy += points[i](1);
+ ++num;
+ }
+
+ double msxx = (sxx - sx * sx / num) / num;
+ double msyy = (syy - sy * sy / num) / num;
+ double msxy = (sxy - sx * sy / num) / num;
+ double b = 2.0 * msxy;
+ double a = msxx;
+ double c = msyy;
+ double theta = 0.5 * (atan2(b, a - c) + M_PI);
+ theta = atan2(sin(theta), cos(theta));
+ double rho = (sx * cos(theta) + sy * sin(theta)) / num;
+
+ if (rho < 0) {
+ theta = atan2(sin(theta + M_PI), cos(theta + M_PI));
+ rho = -rho;
+ }
+ model(0) = cos(theta);
+ model(1) = sin(theta);
+ model(2) = -rho;
+ }
+
+ bool FALKOExtractor::solveSystem2x2(double* A, double* b, double* x) {
+ double det = A[0] * A[3] - A[1] * A[2];
+ if (std::abs(det) < 0.0000001)
+ return false;
+ x[0] = (b[0] * A[3] - A[1] * b[1]) / det;
+ x[1] = (A[0] * b[1] - b[0] * A[2]) / det;
+ return true;
+ }
+}
\ No newline at end of file
diff --git a/src/Feature/OCExtractor.cpp b/src/Feature/OCExtractor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4fce6c24a93f67c48be146759fea6f10774cf3cb
--- /dev/null
+++ b/src/Feature/OCExtractor.cpp
@@ -0,0 +1,293 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <falkolib/Feature/OCExtractor.h>
+#include <cassert>
+
+using namespace std;
+
+namespace falkolib {
+
+ OCExtractor::OCExtractor()
+ //: houghSpectrum((M_PI / 180.0) * 0.5, 0.02, 10.0) {
+ {
+ tol = 0.1;
+ angleRes = (M_PI / 180.0) * 0.5;
+ rangeRes = 0.02;
+ rangeMax = 10.0;
+ nmsRadius = 0.1;
+ neighA = 0.1;
+ neighB = 0.07;
+ neighMinPoint = 2;
+ houghSpectrum.init(angleRes, rangeRes, rangeMax);
+ }
+
+ void OCExtractor::extract(const LaserScan& scan, std::vector<OC>& keypoints) {
+ std::vector<Point2d> pointRot;
+ std::vector<OC> keypointCandidates;
+ std::vector<double> scores;
+ std::vector<int> maxima;
+ OC keypoint;
+ double angle, score;
+ int win;
+ Eigen::Affine2d rot2orth, rot2inve;
+ OC kp;
+
+ // Computes dominant orientation angle and rotates the points
+ angle = computeDominantAngle(scan.points);
+ rotatePoints(scan.points, -angle, pointRot);
+// std::cout << "Rotated " << scan.points.size() << " by angle " << (180.0 / M_PI * angle) << " deg" << std::endl;
+ rot2orth = Eigen::Affine2d::Identity();
+ rot2orth.prerotate(angle);
+ rot2inve = rot2orth.inverse();
+
+ // For each point, computes the corresponding corner and score
+ keypointCandidates.reserve(pointRot.size());
+ scores.reserve(pointRot.size());
+ for (int idx = 0; idx < pointRot.size(); ++idx) {
+ score = computeCornerScore(pointRot, idx, keypoint);
+ keypointCandidates.push_back(keypoint);
+ scores.push_back(score);
+ }
+
+ // Extracts the maxima using non-maxima suppression (NMS)
+ // assert(scores.size() == scan.getNumBeams();
+ NMSKeypoint(scores, scan, 0, scan.getNumBeams(), nmsRadius, 1.0, maxima);
+
+ keypoints.clear();
+ keypoints.reserve(maxima.size());
+ for (auto& idx : maxima) {
+ kp = keypointCandidates[idx];
+ kp.point = rot2orth * keypointCandidates[idx].point;
+ double a = keypointCandidates[idx].orientation + angle;
+ kp.orientation = atan2(sin(a), cos(a));
+ keypoints.push_back(kp);
+ }
+ }
+
+ void OCExtractor::rotatePoints(const std::vector<Point2d>& pointsIn, double angle, std::vector<Point2d>& pointsOut) {
+ Point2d p;
+ double ct = cos(angle);
+ double st = sin(angle);
+
+ pointsOut.clear();
+ pointsOut.reserve(pointsIn.size());
+ for (int i = 0; i < pointsIn.size(); ++i) {
+ p.x() = pointsIn[i].x() * ct - pointsIn[i].y() * st;
+ p.y() = pointsIn[i].x() * st + pointsIn[i].y() * ct;
+ pointsOut.push_back(p);
+ }
+ }
+
+ double OCExtractor::computeDominantAngle(const std::vector<Point2d>& points) {
+ int maxIdx;
+ double angle;
+
+ houghSpectrum.insertPoint(points.begin(), points.end());
+ houghSpectrum.orthoSpectrum().maxCoeff(&maxIdx);
+ angle = angleRes * maxIdx;
+ return angle;
+ }
+
+ double OCExtractor::computeCornerScore(const std::vector<Point2d>& pointsIn, int index, OC& keypoint) {
+ vector<Point2d> neigh;
+ double range, distNeigh;
+ double score, dx, dy;
+ int midIndex, orientationMax;
+ int neighSize, neighSizeL, neighSizeR;
+ int xnum, ynum;
+ int orientationBin[4] = {0, 0, 0, 0};
+
+ // Computes the neighborhood
+ neigh.clear();
+ getNeighPoints(pointsIn, index, neigh, midIndex, distNeigh);
+
+ // Check if it current point is in the center of the neighbor interval
+ neighSize = neigh.size();
+ neighSizeL = midIndex;
+ neighSizeR = neighSize - midIndex - 1;
+ // std::cout << __FILE__ << "," << __LINE__ << ": distNeigh " << distNeigh << ", neighSize " << neighSize << ", neighSizeL " << neighSizeL << ", neighSizeR " << neighSizeR << std::endl;
+ if (neighSizeL < neighMinPoint || neighSizeR < neighMinPoint) {
+ // std::cout << " unbalanced neighborhood: distNeigh " << distNeigh << ", neighSize " << neighSize << ", neighSizeL " << neighSizeL << ", neighSizeR " << neighSizeR << ", neighMinPoint " << neighMinPoint << std::endl;
+ return 0.0;
+ }
+
+ // Check that the neighborhood has a triangular shape rather than a flat one
+ // double triangleBLength = pointsDistance(neigh.front(), neigh.back());
+ // double triangleHLength = std::abs(signedTriangleArea(neigh[midIndex], neigh.front(), neigh.back())) / triangleBLength;
+ // std::cout << __FILE__ << "," << __LINE__ << ": triangleBLength " << triangleBLength << " > " << (distNeigh / bRatio) << ", triangleHLength " << triangleHLength << ", (distNeigh / bRatio) " << (distNeigh / bRatio) << std::endl;
+ // if (triangleBLength < (distNeigh / bRatio) || triangleHLength < (distNeigh / bRatio)) {
+ // return 0.0;
+ // }
+
+ // Computes the keypoint/corner
+ keypoint.index = index;
+ keypoint.point.x() = 0.0;
+ keypoint.point.y() = 0.0;
+ keypoint.index = index;
+ keypoint.radius = distNeigh;
+ xnum = 0;
+ ynum = 0;
+ for (auto& np : neigh) {
+ dx = np.x() - pointsIn[index].x();
+ dy = np.y() - pointsIn[index].y();
+ // a. Neighboor pointIn[i] has similar x coordinate and different y coordinate.
+ // Condition std::abs(dy) < distNeigh means that is inside circle with radius distNeigh.
+ // b. Neighboor pointIn[i] has similar y coordinate and different x coordinate.
+ // Condition std::abs(dx) < distNeigh means that is inside circle with radius distNeigh.
+ if (std::abs(dx) < tol && tol < std::abs(dy) && std::abs(dy) < distNeigh) {
+ xnum++;
+ keypoint.point.x() += (np.x() - keypoint.point.x()) / xnum;
+ if (dy > 0) {
+ orientationBin[NE]++; // NE
+ orientationBin[NW]++; // NW
+ } else {
+ orientationBin[SW]++; // SW
+ orientationBin[SE]++; // SE
+ }
+ }
+ if (std::abs(dy) < tol && tol < std::abs(dx) && std::abs(dx) < distNeigh) {
+ ynum++;
+ keypoint.point.y() += (np.y() - keypoint.point.y()) / ynum;
+ if (dx > 0) {
+ orientationBin[NE]++; // NE
+ orientationBin[SE]++; // SE
+ } else {
+ orientationBin[NW]++; // NW
+ orientationBin[SW]++; // SW
+ }
+ }
+ // std::cout << " index " << index << " dx " << dx << ", dy " << dy << ", xnum " << xnum << ", ynum " << ynum << "\n";
+ }
+ if (xnum == 0) {
+ keypoint.point.x() = pointsIn[index].x();
+ }
+ if (ynum == 0) {
+ keypoint.point.y() = pointsIn[index].y();
+ }
+
+ score = 1.0 * (xnum + ynum) / (0.1 + std::abs(xnum - ynum));
+// if (score > 1.0 || index == 1197 || (1304 <= index && index <= 1305) || (117 <= index && index <= 119)) {
+// std::cout << __FILE__ << "," << __LINE__ << "\n index " << index << ", score " << score << ", xnum " << xnum << ", ynum " << ynum
+// << "\n neighSize " << neighSize << ", neighSizeL " << neighSizeL << ", neighSizeR " << neighSizeR << ", neighMinPoint " << neighMinPoint << std::endl;
+// }
+
+ // Computes keypoint orientation according to maximum of orientationBin[] histogram:
+ // * orientationBin[0]: I quadrant, 45 deg
+ // * orientationBin[1]: II quadrant, 135 deg
+ // * orientationBin[2]: III quadrant, 225 deg (or -135 deg)
+ // * orientationBin[3]: IV quadrant, 315 deg (or -45 deg)
+ orientationMax = 0;
+ for (int i = 1; i < 4; ++i) {
+ if (orientationBin[i] > orientationBin[orientationMax]) {
+ orientationMax = i;
+ }
+ }
+ keypoint.orientation = M_PI * (0.25 + 0.5 * orientationMax);
+ keypoint.orientation = atan2(sin(keypoint.orientation), cos(keypoint.orientation));
+ return score;
+ }
+
+ void OCExtractor::getNeighPoints(const std::vector<Point2d>& pointsIn, int index, std::vector<Point2d>& neigh, int& midIndex, double& dist) const {
+ double range;
+ int imin, imax, win;
+
+ neigh.clear();
+ range = pointsIn[index].norm();
+ dist = neighA * exp(neighB * range);
+ if (range > dist) {
+ win = (int) ceil(asin(dist / range) / angleRes);
+ } else {
+ win = pointsIn.size();
+ }
+ imin = std::max(index - win, 0);
+ imax = std::min(index + win + 1, (int) pointsIn.size());
+ for (int i = imin; i < imax; ++i) {
+ if ((pointsIn[i] - pointsIn[index]).norm() < dist) {
+ neigh.push_back(pointsIn[i]);
+ if (i == index) {
+ midIndex = neigh.size();
+ }
+ }
+ }
+ }
+
+ void OCExtractor::NMSKeypoint(const std::vector<double>& scores, const LaserScan& scan, unsigned int ibeg, unsigned int iend, double radius, int minval, std::vector<int>& peaks) {
+ unsigned i, imax;
+ unsigned j, jbeg, jend, jmax;
+ std::vector<int> candidates;
+ peaks.clear();
+
+ i = ibeg;
+ imax = ibeg;
+ while (i < iend) {
+ int win;
+ if (radius >= scan.ranges[i]) {
+ win = std::floor(std::asin(0.8) / scan.getAngleInc());
+ } else {
+ win = std::floor(std::asin(radius / scan.ranges[i]) / scan.getAngleInc());
+ }
+
+ jmax = i;
+
+ if (imax + win < i) {
+ jbeg = (i >= win ? i - win : 0);
+ imax = i;
+ } else {
+ jbeg = i;
+ }
+
+ jend = (i + win + 1 < iend ? i + win + 1 : iend);
+ for (j = jbeg; j < jend; ++j) {
+ if (scores[j] > scores[jmax]) {
+ jmax = j;
+ }
+ }
+ imax = (scores[jmax] >= scores[imax] ? jmax : imax);
+
+ if (i == imax && scores[i] > minval) {
+ candidates.push_back(i);
+ }
+ if (jmax > i) i = jmax;
+ else ++i;
+ }
+
+ int i1 = 0;
+ int i2 = 0;
+ int counter = 0;
+
+ for (i1 = 0, i2 = 0; i1 < candidates.size(); ++i1) {
+ if (scores[candidates[i2]] == scores[candidates[i1]]) {
+ counter++;
+ if (2 * abs(i2 - i1) > counter) {
+ ++i2;
+ }
+ } else {
+ peaks.push_back(candidates[i2]);
+ i2 = i1;
+ counter = 0;
+ }
+ }
+ if (i2 != candidates.size()) {
+ peaks.push_back(candidates[i2]);
+ }
+ }
+
+}
+
diff --git a/test/testData.cpp b/test/testData.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bce6b18dd853ffa28db4521538110b8e662602bc
--- /dev/null
+++ b/test/testData.cpp
@@ -0,0 +1,24 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+double testRanges[] = {250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 5.62, 5.591, 5.574, 5.595, 5.59, 5.613, 5.605, 5.619, 5.645, 5.678, 5.645, 5.644, 5.677, 5.684, 5.675, 5.675, 5.681, 5.712, 5.751, 5.742, 5.766, 5.807, 5.801, 5.833, 21.32, 21.829, 23.012, 23.046, 23.089, 23.111, 23.164, 23.182, 22.961, 22.104, 21.701, 18.683, 18.667, 250, 18.106, 250, 2.94, 2.874, 2.834, 2.772, 2.745, 2.732, 2.691, 2.66, 2.632, 2.615, 2.614, 2.565, 2.538, 2.527, 2.523, 2.471, 2.447, 2.441, 2.431, 2.421, 2.421, 2.415, 2.381, 2.354, 2.36, 2.344, 2.342, 2.318, 2.335, 2.27, 2.266, 2.253, 2.251, 2.252, 2.212, 2.209, 2.179, 2.188, 2.189, 2.179, 2.178, 2.151, 2.18, 2.169, 2.162, 2.152, 2.125, 2.122, 2.127, 2.121, 2.114, 2.1, 2.118, 2.115, 2.097, 2.13, 2.155, 2.161, 2.167, 2.163, 2.18, 2.174, 2.174, 2.18, 2.222, 2.236, 2.221, 2.206, 2.237, 2.228, 2.229, 2.23, 2.029, 1.905, 1.875, 1.857, 1.846, 1.845, 1.842, 1.827, 1.825, 1.808, 1.797, 1.799, 1.797, 1.813, 1.816, 1.809, 1.831, 1.833, 1.846, 1.891, 1.976, 2.01, 1.999, 2.006, 1.988, 2.01, 2.021, 1.998, 2.011, 2.008, 2.009, 2.002, 1.998, 1.999, 2.006, 1.98, 1.991, 1.998, 1.998, 1.984, 1.984, 1.992, 1.975, 1.973, 1.99, 1.965, 1.991, 1.935, 1.947, 1.967, 1.99, 2, 1.941, 1.775, 1.649, 1.563, 1.524, 1.527, 1.502, 1.521, 1.501, 1.51, 1.515, 1.521, 1.498, 1.493, 1.486, 1.487, 1.479, 1.475, 1.474, 1.479, 1.498, 1.47, 1.471, 1.508, 1.505, 1.521, 1.582, 1.588, 1.611, 1.604, 1.616, 1.632, 1.643, 1.634, 1.631, 1.668, 1.66, 1.654, 1.683, 1.694, 1.668, 1.691, 1.683, 1.68, 1.686, 1.697, 1.691, 1.706, 1.691, 1.694, 1.71, 1.688, 1.708, 1.7, 1.702, 1.695, 1.674, 1.651, 1.659, 1.649, 1.642, 1.63, 1.574, 1.548, 1.489, 1.481, 1.478, 1.42, 1.408, 1.38, 1.356, 1.359, 1.375, 1.336, 1.35, 1.364, 1.36, 1.363, 1.327, 1.341, 1.309, 1.302, 1.315, 1.31, 1.311, 1.319, 1.329, 1.306, 1.305, 1.319, 1.297, 1.316, 1.313, 1.301, 1.298, 1.308, 1.307, 1.281, 1.287, 1.275, 1.314, 1.296, 1.325, 1.304, 1.298, 1.307, 1.291, 1.282, 1.285, 1.281, 1.279, 1.296, 1.284, 1.265, 1.276, 1.283, 1.279, 1.278, 1.284, 1.287, 1.297, 1.27, 1.265, 1.282, 1.285, 1.277, 1.274, 1.264, 1.267, 1.292, 1.323, 1.403, 1.552, 1.622, 1.568, 1.55, 1.508, 1.561, 1.452, 1.644, 1.625, 1.665, 1.634, 1.547, 1.405, 1.293, 1.275, 1.313, 1.359, 1.326, 1.311, 1.339, 1.397, 1.347, 1.402, 1.306, 1.259, 1.256, 1.287, 1.5, 1.512, 1.483, 1.415, 1.446, 1.353, 1.427, 1.397, 1.535, 1.573, 1.614, 1.592, 1.579, 1.645, 1.581, 1.528, 1.607, 1.635, 1.646, 1.728, 1.731, 1.771, 1.765, 1.781, 1.716, 1.672, 1.604, 1.583, 1.6, 1.592, 1.6, 1.577, 1.593, 1.597, 1.586, 1.539, 1.47, 1.389, 1.314, 1.298, 1.317, 1.306, 1.314, 1.311, 1.317, 1.294, 1.313, 1.337, 1.337, 1.317, 1.324, 1.327, 1.331, 1.328, 1.339, 1.33, 1.342, 1.333, 1.353, 1.319, 1.343, 1.351, 1.377, 1.438, 1.592, 1.662, 1.661, 1.66, 1.677, 1.661, 1.731, 1.774, 1.769, 1.844, 1.828, 1.884, 1.873, 1.894, 1.908, 1.892, 1.884, 1.907, 1.943, 1.915, 1.946, 1.946, 1.953, 1.9, 1.854, 1.85, 1.834, 1.844, 1.805, 1.817, 1.796, 1.795, 1.776, 1.786, 1.792, 1.786, 1.766, 1.786, 1.779, 1.783, 1.768, 1.774, 1.757, 1.741, 1.733, 1.702, 1.659, 1.62, 1.57, 1.578, 1.563, 1.542, 1.547, 1.537, 1.521, 1.544, 1.54, 1.535, 1.523, 1.579, 1.595, 1.598, 1.615, 1.63, 1.635, 1.623, 1.597, 1.621, 1.61, 1.615, 1.709, 2.039, 2.079, 2.065, 2.055, 2.04, 2.048, 2.048, 2.031, 2.053, 2.015, 2.001, 1.996, 1.963, 1.955, 1.911, 1.866, 1.835, 1.788, 1.719, 1.689, 1.653, 1.511, 1.47, 1.475, 1.474, 1.458, 1.48, 1.468, 1.459, 1.468, 1.468, 1.461, 1.468, 1.479, 1.465, 1.453, 1.479, 1.479, 1.48, 1.521, 1.529, 1.541, 1.56, 1.604, 1.627, 1.729, 1.735, 1.771, 1.805, 1.81, 1.828, 1.854, 1.815, 1.848, 1.86, 1.878, 1.885, 1.902, 1.928, 1.923, 1.931, 1.946, 1.958, 1.943, 1.961, 1.981, 1.977, 1.985, 2.005, 2.016, 2.044, 2.031, 2.052, 2.065, 2.071, 2.08, 2.105, 2.105, 2.128, 2.135, 2.149, 2.183, 2.159, 2.186, 2.188, 2.197, 2.207, 2.223, 2.24, 2.256, 2.282, 2.297, 2.311, 2.341, 2.351, 2.383, 2.41, 2.42, 2.45, 2.461, 2.477, 2.511, 2.529, 2.547, 2.571, 2.561, 2.534, 2.552, 2.617, 2.724, 2.812, 2.801, 2.801, 2.789, 2.8, 2.811, 2.81, 2.815, 2.82, 2.84, 2.867, 2.889, 2.901, 2.914, 2.924, 2.946, 3.005, 3.056, 3.051, 3.093, 3.092, 3.14, 3.216, 3.213, 3.2, 3.211, 3.23, 3.242, 3.27, 3.323, 3.357, 3.424, 12.108, 12.065, 12.075, 12.058, 12.023, 12.015, 11.996, 11.981, 11.986, 11.953, 11.959, 11.937, 11.909, 11.903, 11.899, 11.873, 11.882, 11.846, 11.842, 11.808, 11.789, 11.816, 11.815, 11.771, 11.764, 11.766, 11.749, 11.741, 11.732, 11.723, 11.726, 11.707, 11.715, 11.692, 11.7, 11.667, 11.672, 11.673, 11.659, 11.655, 11.654, 11.643, 11.633, 11.64, 11.639, 11.606, 11.619, 11.612, 11.604, 11.608, 11.602, 11.605, 11.593, 11.587, 11.583, 11.581, 11.588, 11.577, 11.567, 11.567, 11.579, 11.577, 11.563, 11.556, 11.41, 11.433, 11.453, 11.558, 11.493, 11.565, 11.568, 11.552, 11.553, 11.56, 11.574, 11.584, 11.561, 11.572, 11.577, 11.568, 11.583, 11.586, 11.586, 11.579, 11.595, 11.591, 11.605, 11.59, 11.616, 11.612, 11.616, 11.631, 11.639, 11.652, 11.654, 11.668, 11.657, 11.651, 11.667, 11.69, 11.688, 11.71, 11.692, 11.712, 11.726, 11.728, 11.736, 11.735, 11.755, 11.77, 11.768, 11.788, 11.791, 11.798, 11.794, 11.814, 11.841, 11.825, 11.856, 11.857, 11.863, 11.906, 11.91, 11.914, 11.931, 11.955, 11.965, 11.97, 11.98, 12.006, 12.016, 12.027, 12.044, 12.067, 12.082, 12.115, 12.129, 12.136, 12.151, 250, 3.819, 3.839, 3.819, 3.848, 3.832, 3.837, 3.839, 3.813, 3.817, 3.811, 3.815, 3.82, 3.873, 3.875, 3.885, 3.884, 3.834, 3.848, 3.824, 3.689, 3.714, 3.636, 3.564, 3.493, 3.423, 3.41, 3.355, 3.323, 3.293, 3.275, 3.226, 3.196, 3.188, 3.145, 3.123, 3.112, 3.093, 3.057, 3.005, 3.002, 3, 2.942, 2.908, 2.888, 2.89, 2.864, 2.848, 2.815, 2.789, 2.79, 2.776, 2.772, 2.772, 2.725, 2.713, 2.704, 2.704, 2.695, 2.695, 2.704, 2.704, 2.712, 2.706, 2.604, 2.562, 2.543, 2.526, 2.48, 2.469, 2.453, 2.441, 2.443, 2.428, 2.401, 2.368, 2.36, 2.349, 2.346, 2.339, 2.314, 2.309, 2.283, 2.277, 2.267, 2.255, 2.264, 2.225, 2.215, 2.194, 2.191, 2.175, 2.172, 2.149, 2.168, 2.142, 2.142, 2.128, 2.08, 2.073, 2.064, 2.068, 2.068, 2.06, 2.062, 2.042, 2.021, 2.002, 2.002, 1.991, 1.986, 1.98, 1.957, 1.981, 1.992, 1.954, 1.937, 1.918, 1.913, 1.895, 1.881, 1.891, 1.879, 1.884, 1.905, 1.886, 1.872, 1.85, 1.852, 1.828, 1.842, 1.835, 1.825, 1.8, 1.821, 1.828, 1.828, 1.831, 1.81, 1.804, 1.779, 1.767, 1.781, 1.768, 1.771, 1.771, 1.759, 1.751, 1.771, 1.761, 1.777, 1.776, 1.776, 1.803, 1.82, 1.838, 1.866, 1.79, 1.728, 1.699, 1.685, 1.671, 1.66, 1.67, 1.656, 1.661, 1.666, 1.652, 1.63, 1.638, 1.641, 1.627, 1.623, 1.635, 1.626, 1.617, 1.613, 1.603, 1.61, 1.61, 1.599, 1.587, 1.584, 1.583, 1.584, 1.579, 1.567, 1.569, 1.56, 1.538, 1.565, 1.542, 1.536, 1.567, 1.544, 1.551, 1.547, 1.516, 1.524, 1.511, 1.523, 1.522, 1.514, 1.516, 1.492, 1.504, 1.509, 1.482, 1.493, 1.485, 1.499, 1.496, 1.482, 1.48, 1.475, 1.473, 1.49, 1.473, 1.482, 1.463, 1.467, 1.459, 1.469, 1.453, 1.455, 1.469, 1.458, 1.468, 1.452, 1.463, 1.449, 1.457, 1.447, 1.439, 1.439, 1.46, 1.439, 1.44, 1.438, 1.435, 1.44, 1.445, 1.426, 1.425, 1.432, 1.427, 1.415, 1.432, 1.424, 1.412, 1.425, 1.428, 1.438, 1.418, 1.423, 1.436, 1.431, 1.426, 1.426, 1.447, 1.419, 1.427, 1.42, 1.43, 1.41, 1.439, 1.409, 1.422, 1.435, 1.435, 1.415, 1.39, 1.4, 1.426, 1.412, 1.419, 1.397, 1.418, 1.403, 1.411, 1.434, 1.414, 1.419, 1.409, 1.427, 1.414, 1.417, 1.417, 1.411, 1.435, 1.409, 1.422, 1.416, 1.432, 1.425, 1.431, 1.423, 1.431, 1.418, 1.439, 1.412, 1.422, 1.428, 1.433, 1.418, 1.434, 1.437, 1.44, 1.432, 1.437, 10.855, 10.857, 10.856, 10.86, 10.881, 10.889, 10.885, 10.873, 10.904, 10.911, 10.921, 10.92, 10.933, 250, 2.356, 2.316, 2.248, 2.195, 2.139, 2.097, 2.063, 2.012, 1.985, 1.924, 1.899, 1.869, 1.824, 1.813, 1.803, 1.763, 1.747, 1.719, 1.709, 1.704, 1.688, 1.639, 1.646, 1.609, 1.588, 1.578, 1.567, 1.549, 1.52, 1.492, 1.506, 1.528, 1.501, 1.532, 1.532, 1.532, 1.531, 1.526, 1.542, 1.542, 1.542, 1.54, 1.538, 1.557, 1.552, 1.55, 1.546, 1.562, 1.553, 1.567, 1.566, 1.566, 1.572, 1.551, 1.591, 1.596, 1.599, 1.593, 1.604, 1.598, 1.595, 1.585, 1.596, 1.62, 1.594, 1.633, 1.631, 1.613, 1.624, 1.63, 1.642, 1.635, 1.654, 1.646, 1.662, 1.648, 1.649, 1.659, 1.658, 1.688, 1.681, 1.688, 1.694, 1.678, 1.695, 1.689, 1.686, 1.704, 1.699, 1.719, 1.724, 1.731, 1.729, 1.734, 1.744, 1.753, 1.75, 1.777, 1.78, 1.773, 1.779, 1.778, 1.776, 1.799, 1.794, 1.813, 1.808, 1.822, 1.808, 1.836, 1.822, 1.835, 1.845, 1.855, 1.856, 1.872, 1.866, 1.888, 1.882, 1.891, 1.913, 1.912, 1.927, 1.916, 1.92, 1.938, 1.934, 1.977, 1.972, 1.974, 1.97, 1.998, 2.001, 2.01, 2.031, 2.022, 2.039, 2.054, 2.044, 2.076, 2.077, 2.105, 2.134, 2.193, 2.2, 2.187, 2.16, 2.184, 2.176, 2.174, 2.161, 2.169, 2.143, 2.148, 2.143, 2.136, 2.118, 2.127, 2.109, 2.141, 2.136, 2.146, 2.131, 2.159, 2.139, 2.167, 2.195, 2.228, 2.215, 2.214, 2.232, 2.25, 2.276, 2.306, 2.305, 2.324, 2.33, 2.342, 2.379, 2.406, 2.426, 2.425, 2.426, 2.446, 2.464, 2.488, 2.519, 2.522, 2.554, 2.559, 2.581, 2.613, 2.634, 2.651, 2.652, 2.684, 2.731, 2.751, 2.761, 2.775, 2.814, 2.842, 2.867, 2.887, 2.922, 2.966, 3.01, 3.118, 3.139, 3.139, 3.147, 3.143, 3.118, 3.123, 3.122, 3.134, 3.154, 3.157, 3.195, 3.221, 3.232, 3.263, 3.306, 3.349, 3.443, 3.489, 3.479, 3.492, 3.505, 3.519, 3.534, 3.607, 3.725, 3.822, 3.886, 3.922, 3.991, 4.063, 4.126, 4.194, 4.253, 4.291, 4.289, 4.293, 4.271, 4.265, 4.251, 4.223, 4.246, 4.243, 4.246, 4.267, 4.242, 4.271, 250, 15.705, 15.89, 16.172, 16.401, 16.571, 16.903, 17.157, 17.337, 17.702, 17.94, 18.313, 18.526, 18.878, 19.159, 19.659, 19.896, 20.309, 20.669, 21.071, 21.554, 22.006, 21.988, 22.021, 22.134, 22.148, 22.127, 22.098, 22.077, 22.042, 22.033, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250};
+double testRanges2[] = {250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 5.617, 5.593, 5.58, 5.579, 5.605, 5.628, 5.613, 5.633, 5.662, 5.642, 5.646, 5.655, 5.663, 5.682, 5.677, 5.672, 5.691, 5.729, 5.747, 5.733, 5.783, 5.798, 5.8, 5.835, 21.331, 21.854, 23.03, 23.055, 23.082, 23.133, 23.178, 23.174, 22.968, 22.127, 21.706, 18.682, 18.669, 250, 18.153, 2.957, 2.961, 2.878, 2.842, 2.776, 2.735, 2.718, 2.69, 2.659, 2.625, 2.627, 2.612, 2.562, 2.549, 2.534, 2.52, 2.474, 2.441, 2.44, 2.428, 2.424, 2.422, 2.407, 2.379, 2.364, 2.354, 2.343, 2.33, 2.312, 2.293, 2.271, 2.248, 2.251, 2.257, 2.252, 2.241, 2.194, 2.188, 2.197, 2.187, 2.174, 2.164, 2.163, 2.167, 2.177, 2.176, 2.144, 2.125, 2.126, 2.125, 2.113, 2.103, 2.109, 2.107, 2.121, 2.119, 2.11, 2.153, 2.168, 2.17, 2.156, 2.184, 2.18, 2.182, 2.193, 2.238, 2.228, 2.239, 2.22, 2.226, 2.242, 2.229, 2.222, 1.994, 1.884, 1.86, 1.85, 1.846, 1.849, 1.856, 1.838, 1.829, 1.813, 1.812, 1.794, 1.808, 1.81, 1.824, 1.825, 1.827, 1.836, 1.868, 1.906, 1.987, 2.003, 1.998, 2.012, 2.016, 2.022, 2.003, 2, 2.008, 2.012, 2.018, 2.011, 1.999, 1.999, 2.018, 1.997, 1.989, 2.001, 2.002, 1.995, 1.981, 1.988, 1.971, 1.988, 2.001, 1.984, 1.961, 1.936, 1.951, 1.992, 1.979, 1.998, 1.866, 1.714, 1.618, 1.533, 1.528, 1.521, 1.54, 1.533, 1.514, 1.503, 1.514, 1.497, 1.495, 1.497, 1.508, 1.485, 1.478, 1.474, 1.479, 1.475, 1.463, 1.487, 1.479, 1.523, 1.495, 1.542, 1.559, 1.595, 1.602, 1.614, 1.618, 1.614, 1.64, 1.625, 1.638, 1.656, 1.675, 1.686, 1.678, 1.678, 1.682, 1.674, 1.716, 1.703, 1.663, 1.672, 1.693, 1.73, 1.697, 1.669, 1.69, 1.702, 1.646, 1.698, 1.676, 1.681, 1.669, 1.665, 1.683, 1.659, 1.655, 1.623, 1.54, 1.522, 1.513, 1.474, 1.439, 1.412, 1.387, 1.397, 1.394, 1.361, 1.348, 1.343, 1.354, 1.361, 1.374, 1.362, 1.321, 1.313, 1.312, 1.328, 1.315, 1.328, 1.314, 1.316, 1.298, 1.309, 1.305, 1.321, 1.319, 1.303, 1.313, 1.284, 1.295, 1.309, 1.303, 1.305, 1.3, 1.309, 1.284, 1.294, 1.32, 1.297, 1.316, 1.287, 1.285, 1.279, 1.311, 1.286, 1.285, 1.288, 1.293, 1.299, 1.291, 1.28, 1.283, 1.287, 1.282, 1.269, 1.279, 1.264, 1.282, 1.286, 1.264, 1.285, 1.286, 1.273, 1.286, 1.275, 1.345, 1.438, 1.614, 1.599, 1.557, 1.582, 1.545, 1.556, 1.508, 1.688, 1.635, 1.659, 1.593, 1.534, 1.414, 1.299, 1.29, 1.315, 1.353, 1.322, 1.306, 1.32, 1.414, 1.421, 1.395, 1.271, 1.25, 1.271, 1.386, 1.51, 1.532, 1.437, 1.424, 1.41, 1.349, 1.422, 1.474, 1.549, 1.565, 1.549, 1.562, 1.59, 1.591, 1.569, 1.523, 1.621, 1.651, 1.696, 1.728, 1.74, 1.744, 1.713, 1.744, 1.709, 1.683, 1.593, 1.601, 1.588, 1.586, 1.579, 1.587, 1.584, 1.599, 1.591, 1.517, 1.454, 1.352, 1.326, 1.321, 1.302, 1.31, 1.325, 1.321, 1.297, 1.323, 1.305, 1.323, 1.306, 1.315, 1.306, 1.323, 1.335, 1.342, 1.332, 1.345, 1.339, 1.327, 1.34, 1.336, 1.357, 1.36, 1.369, 1.507, 1.637, 1.653, 1.649, 1.646, 1.668, 1.693, 1.738, 1.779, 1.829, 1.852, 1.857, 1.851, 1.896, 1.906, 1.906, 1.878, 1.914, 1.911, 1.952, 1.919, 1.934, 1.945, 1.932, 1.903, 1.863, 1.837, 1.835, 1.817, 1.812, 1.81, 1.801, 1.783, 1.799, 1.801, 1.778, 1.794, 1.782, 1.776, 1.767, 1.768, 1.783, 1.756, 1.732, 1.737, 1.724, 1.685, 1.641, 1.586, 1.586, 1.572, 1.557, 1.557, 1.521, 1.534, 1.51, 1.541, 1.545, 1.541, 1.566, 1.583, 1.581, 1.602, 1.627, 1.618, 1.63, 1.614, 1.613, 1.593, 1.603, 1.629, 1.745, 2.084, 2.077, 2.066, 2.07, 2.029, 2.044, 2.043, 2.042, 2.043, 2.001, 1.987, 1.982, 1.958, 1.94, 1.893, 1.855, 1.799, 1.731, 1.693, 1.701, 1.627, 1.502, 1.467, 1.476, 1.484, 1.462, 1.475, 1.475, 1.466, 1.471, 1.448, 1.453, 1.457, 1.455, 1.451, 1.453, 1.483, 1.491, 1.511, 1.53, 1.545, 1.553, 1.556, 1.607, 1.646, 1.738, 1.756, 1.782, 1.804, 1.811, 1.829, 1.844, 1.861, 1.871, 1.859, 1.867, 1.859, 1.896, 1.895, 1.917, 1.923, 1.936, 1.931, 1.952, 1.962, 1.964, 1.995, 2.01, 2.01, 2.023, 2.025, 2.05, 2.054, 2.051, 2.08, 2.096, 2.105, 2.094, 2.114, 2.143, 2.149, 2.155, 2.163, 2.173, 2.17, 2.201, 2.193, 2.23, 2.232, 2.262, 2.282, 2.299, 2.318, 2.33, 2.349, 2.401, 2.417, 2.431, 2.45, 2.478, 2.492, 2.512, 2.523, 2.544, 2.561, 2.547, 2.541, 2.554, 2.636, 2.774, 2.804, 2.803, 2.809, 2.797, 2.795, 2.799, 2.801, 2.809, 2.814, 2.859, 2.854, 2.892, 2.912, 2.921, 2.922, 2.957, 3.024, 3.059, 3.061, 3.082, 3.111, 3.14, 3.196, 3.204, 3.2, 3.225, 3.225, 3.23, 3.267, 3.332, 3.366, 3.449, 12.079, 12.094, 12.04, 12.029, 12.045, 12.008, 12.012, 11.979, 11.982, 11.948, 11.944, 11.937, 11.936, 11.908, 11.894, 11.89, 11.866, 11.859, 11.846, 11.831, 11.785, 11.814, 11.803, 11.799, 11.784, 11.771, 11.761, 11.765, 11.732, 11.734, 11.708, 11.716, 11.701, 11.706, 11.678, 11.683, 11.666, 11.654, 11.654, 11.646, 11.651, 11.636, 11.641, 11.633, 11.626, 11.638, 11.613, 11.626, 11.627, 11.585, 11.612, 11.605, 11.595, 11.576, 11.576, 11.595, 11.577, 11.563, 11.562, 11.577, 11.574, 11.573, 11.569, 11.543, 11.429, 11.425, 11.447, 11.563, 11.495, 11.558, 11.548, 11.541, 11.566, 11.556, 11.554, 11.576, 11.587, 11.567, 11.564, 11.591, 11.572, 11.579, 11.581, 11.574, 11.583, 11.578, 11.587, 11.601, 11.631, 11.625, 11.631, 11.606, 11.64, 11.646, 11.623, 11.637, 11.658, 11.673, 11.666, 11.657, 11.698, 11.701, 11.69, 11.703, 11.691, 11.726, 11.73, 11.736, 11.752, 11.753, 11.779, 11.771, 11.775, 11.799, 11.805, 11.801, 11.832, 11.825, 11.865, 11.872, 11.878, 11.898, 11.9, 11.928, 11.925, 11.945, 11.957, 11.968, 11.988, 11.986, 12.018, 12.037, 12.048, 12.07, 12.091, 12.095, 12.11, 12.132, 12.135, 3.819, 3.835, 3.827, 3.785, 3.822, 3.853, 3.851, 3.823, 3.818, 3.81, 3.805, 3.812, 3.834, 3.867, 3.888, 3.896, 3.878, 3.848, 3.877, 3.785, 3.689, 3.709, 3.624, 3.545, 3.493, 3.421, 3.39, 3.375, 3.308, 3.283, 3.267, 3.225, 3.218, 3.192, 3.171, 3.116, 3.089, 3.082, 3.048, 3.017, 2.985, 2.975, 2.949, 2.906, 2.896, 2.888, 2.864, 2.854, 2.811, 2.787, 2.779, 2.78, 2.768, 2.757, 2.739, 2.723, 2.701, 2.704, 2.698, 2.688, 2.693, 2.707, 2.704, 2.704, 2.612, 2.562, 2.548, 2.548, 2.489, 2.454, 2.447, 2.436, 2.445, 2.44, 2.396, 2.384, 2.35, 2.366, 2.348, 2.334, 2.317, 2.302, 2.27, 2.28, 2.274, 2.267, 2.25, 2.23, 2.215, 2.189, 2.19, 2.183, 2.181, 2.167, 2.17, 2.148, 2.146, 2.14, 2.073, 2.089, 2.073, 2.073, 2.078, 2.071, 2.054, 2.042, 2.002, 1.997, 1.993, 1.975, 1.988, 1.978, 1.981, 1.953, 1.966, 1.955, 1.936, 1.919, 1.91, 1.923, 1.906, 1.885, 1.907, 1.892, 1.885, 1.881, 1.875, 1.853, 1.836, 1.848, 1.823, 1.819, 1.805, 1.821, 1.822, 1.821, 1.841, 1.815, 1.814, 1.782, 1.804, 1.778, 1.771, 1.761, 1.766, 1.778, 1.766, 1.761, 1.77, 1.771, 1.772, 1.775, 1.799, 1.813, 1.801, 1.83, 1.842, 1.832, 1.742, 1.686, 1.677, 1.675, 1.685, 1.673, 1.653, 1.679, 1.654, 1.661, 1.652, 1.636, 1.637, 1.649, 1.63, 1.62, 1.6, 1.604, 1.624, 1.602, 1.576, 1.604, 1.59, 1.581, 1.586, 1.575, 1.57, 1.563, 1.566, 1.559, 1.567, 1.556, 1.547, 1.537, 1.528, 1.54, 1.527, 1.53, 1.544, 1.521, 1.528, 1.501, 1.517, 1.516, 1.525, 1.511, 1.522, 1.514, 1.485, 1.496, 1.512, 1.511, 1.505, 1.474, 1.495, 1.49, 1.484, 1.497, 1.485, 1.486, 1.478, 1.459, 1.473, 1.445, 1.467, 1.482, 1.479, 1.452, 1.447, 1.467, 1.476, 1.434, 1.46, 1.459, 1.432, 1.445, 1.44, 1.441, 1.45, 1.447, 1.432, 1.442, 1.449, 1.424, 1.439, 1.439, 1.431, 1.433, 1.438, 1.44, 1.427, 1.414, 1.418, 1.44, 1.423, 1.417, 1.416, 1.437, 1.43, 1.419, 1.435, 1.424, 1.424, 1.404, 1.425, 1.413, 1.417, 1.415, 1.439, 1.438, 1.437, 1.443, 1.416, 1.426, 1.416, 1.419, 1.404, 1.41, 1.408, 1.399, 1.414, 1.424, 1.419, 1.413, 1.425, 1.413, 1.407, 1.417, 1.419, 1.436, 1.414, 1.436, 1.421, 1.424, 1.435, 1.433, 1.439, 1.412, 1.42, 1.433, 1.431, 1.435, 1.44, 1.429, 1.406, 1.414, 1.436, 1.433, 1.438, 1.431, 1.436, 1.446, 1.422, 10.867, 10.876, 10.859, 10.867, 10.874, 10.889, 10.882, 10.903, 10.902, 10.921, 10.924, 10.919, 10.93, 2.371, 2.324, 2.277, 2.216, 2.166, 2.121, 2.081, 2.007, 1.988, 1.943, 1.894, 1.884, 1.859, 1.806, 1.787, 1.771, 1.744, 1.747, 1.733, 1.689, 1.705, 1.657, 1.648, 1.642, 1.59, 1.598, 1.563, 1.557, 1.521, 1.525, 1.533, 1.5, 1.512, 1.519, 1.503, 1.525, 1.544, 1.536, 1.522, 1.543, 1.557, 1.536, 1.536, 1.551, 1.565, 1.555, 1.562, 1.56, 1.546, 1.565, 1.567, 1.565, 1.583, 1.583, 1.563, 1.606, 1.583, 1.604, 1.579, 1.621, 1.615, 1.604, 1.613, 1.614, 1.605, 1.628, 1.626, 1.604, 1.629, 1.618, 1.635, 1.626, 1.645, 1.638, 1.667, 1.667, 1.661, 1.663, 1.658, 1.664, 1.676, 1.661, 1.681, 1.69, 1.692, 1.706, 1.696, 1.724, 1.702, 1.701, 1.738, 1.709, 1.744, 1.732, 1.738, 1.754, 1.772, 1.744, 1.771, 1.771, 1.786, 1.76, 1.769, 1.807, 1.792, 1.804, 1.81, 1.841, 1.824, 1.837, 1.853, 1.833, 1.842, 1.87, 1.866, 1.849, 1.883, 1.884, 1.862, 1.896, 1.902, 1.927, 1.926, 1.94, 1.948, 1.935, 1.952, 1.946, 1.973, 1.979, 1.99, 2.008, 1.998, 2.021, 2.03, 2.038, 2.059, 2.047, 2.047, 2.069, 2.094, 2.101, 2.135, 2.197, 2.189, 2.184, 2.182, 2.185, 2.188, 2.186, 2.184, 2.165, 2.153, 2.136, 2.128, 2.145, 2.131, 2.129, 2.116, 2.132, 2.143, 2.134, 2.146, 2.157, 2.146, 2.17, 2.22, 2.215, 2.223, 2.228, 2.239, 2.269, 2.261, 2.288, 2.307, 2.312, 2.327, 2.336, 2.379, 2.405, 2.428, 2.428, 2.443, 2.44, 2.463, 2.484, 2.508, 2.533, 2.538, 2.564, 2.588, 2.599, 2.644, 2.65, 2.663, 2.683, 2.748, 2.753, 2.762, 2.77, 2.814, 2.847, 2.863, 2.875, 2.9, 2.968, 2.999, 3.117, 3.142, 3.132, 3.126, 3.139, 3.124, 3.123, 3.126, 3.116, 3.163, 3.188, 3.207, 3.214, 3.246, 3.284, 3.308, 3.353, 3.438, 3.483, 3.466, 3.467, 3.497, 3.521, 3.554, 3.595, 3.718, 3.816, 3.88, 3.904, 4.006, 4.061, 4.116, 4.159, 4.244, 4.305, 4.303, 4.293, 4.267, 4.258, 4.232, 4.237, 4.246, 4.243, 4.28, 4.23, 4.266, 4.301, 250, 15.708, 15.894, 16.17, 16.392, 16.558, 16.899, 17.175, 17.345, 17.708, 17.955, 18.299, 18.532, 18.88, 19.168, 19.657, 19.872, 20.318, 20.668, 21.067, 21.543, 21.989, 22.006, 22.029, 22.138, 22.123, 22.104, 22.103, 22.064, 22.064, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250};
+double testRangesOrtho1[] = {7.88663, 7.89185, 7.89723, 7.90276, 7.90846, 7.91431, 7.92033, 7.9265, 7.93284, 7.93933, 7.94599, 7.95281, 7.9598, 7.96694, 7.97426, 7.98174, 7.98938, 7.99719, 8.00517, 8.01332, 8.02164, 8.03013, 8.03879, 8.04762, 8.05662, 8.0658, 8.07515, 8.08468, 8.09439, 8.10427, 8.11434, 8.12458, 8.135, 8.14561, 8.1564, 8.16737, 8.17853, 8.18987, 8.20141, 8.21313, 8.22504, 8.23714, 8.24944, 8.26193, 8.27462, 8.2875, 8.30059, 8.31387, 8.32735, 8.34104, 8.35493, 8.36903, 8.38333, 8.39785, 8.41257, 8.42751, 8.44266, 8.45803, 8.47361, 8.48942, 8.50544, 8.52169, 8.53816, 8.55486, 8.57179, 8.58895, 8.60635, 8.62398, 8.64184, 8.65995, 8.6783, 8.69689, 8.71572, 8.73481, 8.75414, 8.77373, 8.79358, 8.81368, 8.83405, 8.85467, 8.87557, 8.89673, 8.91816, 8.93987, 8.96185, 8.98412, 9.00666, 9.0295, 9.05262, 9.07603, 9.09974, 9.12375, 9.14806, 9.17267, 9.19759, 9.22283, 9.24837, 9.27424, 9.30043, 9.32695, 9.3538, 9.38098, 9.4085, 9.43636, 9.46457, 9.49313, 9.52204, 9.55131, 9.58095, 9.61095, 9.64133, 9.67208, 9.70322, 9.73474, 9.76666, 9.79897, 9.83168, 9.86481, 9.89834, 9.9323, 9.96667, 10.0015, 10.0108, 9.95724, 9.90444, 9.85239, 9.80106, 9.75045, 9.70054, 9.65133, 9.60279, 9.55493, 9.50771, 9.46114, 9.4152, 9.36989, 9.32518, 9.28107, 9.23756, 9.19462, 9.15226, 9.11046, 9.06921, 9.0285, 8.98832, 8.94868, 8.90955, 7.46284, 7.43077, 7.39911, 7.36787, 7.33702, 7.30657, 7.27651, 7.24684, 7.21754, 7.18861, 7.16005, 7.13185, 7.10401, 7.07652, 7.04937, 7.02257, 6.9961, 6.96996, 6.94415, 6.91866, 6.89349, 6.86863, 6.84407, 6.81983, 6.79588, 6.77223, 6.74887, 6.7258, 6.70301, 6.6805, 6.65827, 6.63632, 6.61463, 6.59321, 6.57206, 6.55116, 6.53052, 6.51013, 6.49, 6.47011, 6.45046, 6.43105, 6.41189, 6.39296, 6.37426, 6.35579, 6.33754, 6.31953, 6.30173, 6.28415, 6.26679, 6.24965, 6.23271, 6.21599, 6.19947, 6.18316, 6.16705, 6.15114, 6.13543, 6.11991, 6.10459, 6.08946, 6.07453, 6.05978, 6.04521, 6.03083, 6.01664, 6.00262, 5.98878, 5.97512, 5.96164, 5.94833, 5.93519, 5.92222, 5.90942, 5.89679, 5.88432, 5.87202, 5.85988, 5.8479, 5.83608, 5.82442, 5.81292, 5.80157, 5.79038, 5.77934, 5.76845, 5.75771, 5.74712, 5.73668, 5.72638, 5.71623, 5.70623, 5.69637, 5.68665, 5.67707, 5.66763, 5.65833, 5.64917, 5.64014, 5.63125, 5.6225, 5.61388, 5.60539, 5.59703, 5.58881, 5.58071, 5.57275, 5.56491, 5.5572, 5.54962, 5.54217, 5.53484, 5.52763, 5.52055, 5.51359, 5.50675, 5.50003, 5.49344, 5.48696, 5.48061, 5.47437, 5.46825, 5.46225, 5.45637, 5.4506, 5.44495, 5.43941, 5.43399, 5.42868, 5.42349, 5.41841, 5.41344, 5.40858, 5.40383, 5.3992, 5.39467, 5.39026, 5.38596, 5.38176, 5.37767, 5.3737, 5.36983, 5.36607, 5.36241, 5.35886, 5.35542, 5.35209, 5.34886, 5.34573, 5.34272, 5.3398, 5.33699, 5.33429, 5.33169, 5.32919, 5.3268, 5.32451, 5.32233, 5.32025, 5.31827, 5.31639, 5.31461, 5.31294, 5.31137, 5.3099, 5.30854, 5.30727, 5.30611, 5.30505, 5.30409, 5.30323, 5.30247, 5.30182, 5.30126, 5.30081, 5.30045, 5.3002, 5.30005, 5.3, 5.30005, 5.3002, 5.30045, 5.30081, 5.30126, 5.30182, 5.30247, 5.30323, 5.30409, 5.30505, 5.30611, 5.30727, 5.30854, 5.3099, 5.31137, 5.31294, 5.31461, 5.31639, 5.31827, 5.32025, 5.32233, 5.32451, 5.3268, 5.32919, 5.33169, 5.33429, 5.33699, 5.3398, 5.34272, 5.34573, 5.34886, 5.35209, 5.35542, 5.35886, 5.36241, 5.36607, 5.36983, 5.3737, 5.37767, 5.38176, 5.38596, 5.39026, 5.39467, 5.3992, 5.40383, 5.40858, 5.41344, 5.41841, 5.42349, 5.42868, 5.43399, 5.43941, 5.44495, 5.4506, 5.45637, 5.46225, 5.46825, 5.47437, 5.48061, 5.48696, 5.49344, 5.50003, 5.50675, 5.51359, 5.52055, 5.52763, 5.53484, 5.54217, 5.54962, 5.5572, 5.56491, 5.57275, 5.58071, 5.58881, 5.59703, 5.60539, 5.61388, 5.6225, 5.63125, 5.64014, 5.64917, 5.65833, 5.66763, 5.67707, 5.68665, 5.69637, 5.70623, 5.71623, 5.72638, 5.73668, 5.74712, 5.75771, 5.76845, 5.77934, 5.79038, 5.80157, 5.81292, 5.82442, 5.83608, 5.8479, 5.85988, 5.87202, 5.88432, 5.89679, 5.90942, 5.92222, 5.93519, 5.94833, 5.96164, 5.97512, 5.98878, 6.00262, 6.01664, 6.03083, 6.04521, 6.05978, 6.07453, 6.08946, 6.10459, 6.11991, 6.13543, 6.15114, 6.16705, 6.18316, 6.19947, 6.21599, 6.23271, 6.24965, 6.26679, 6.28415, 6.30173, 6.31953, 6.33754, 6.35579, 6.37426, 6.39296, 6.41189, 6.43105, 6.45046, 6.47011, 6.49, 6.51013, 6.53052, 6.55116, 6.57206, 6.59321, 6.61463, 6.63632, 6.65827, 6.6805, 6.70301, 6.7258, 6.74887, 6.77223, 6.79588, 6.81983, 6.84407, 6.86863, 6.89349, 6.91866, 6.94415, 6.96996, 6.9961, 7.02257, 7.04937, 7.07652, 7.10401, 7.13185, 7.16005, 7.18861, 7.21754, 7.24684, 7.27651, 7.30657, 7.33702, 7.36787, 7.39911, 7.43077, 7.46284, 7.49533, 7.52825, 7.56161, 7.5954, 7.62965, 7.66435, 7.69952, 7.73516, 7.77128, 7.76226, 7.73115, 7.70043, 7.67011, 7.64016, 7.6106, 7.5814, 7.55257, 7.52411, 7.496, 7.46824, 7.44082, 7.41375, 7.38701, 7.36061, 7.33453, 7.30877, 7.28334, 7.25822, 7.2334, 7.2089, 7.18469, 7.16079, 7.13717, 7.11385, 7.09081, 7.06806, 7.04559, 7.02339, 7.00146, 6.97981, 6.95842, 6.93729, 6.91642, 6.8958, 6.87544, 6.85533, 6.83547, 6.81585, 6.79647, 6.77733, 6.75843, 6.73976, 6.72132, 6.7031, 6.68512, 6.66735, 6.64981, 6.63248, 6.61537, 6.59848, 6.58179, 6.56532, 6.54905, 6.53298, 6.51712, 6.50146, 6.48599, 6.47072, 6.45565, 6.44077, 6.42608, 6.41157, 6.39726, 6.38313, 6.36918, 6.35542, 6.34183, 6.32842, 6.31519, 6.30214, 6.28925, 6.27654, 6.264, 6.25163, 6.23943, 6.22739, 6.21551, 6.2038, 6.19225, 6.18087, 6.16964, 6.15856, 6.14765, 6.13689, 6.12628, 6.11583, 6.10553, 6.09538, 6.08537, 6.07552, 6.06581, 6.05625, 6.04684, 6.03757, 6.02844, 6.01945, 6.01061, 6.0019, 5.99333, 5.98491, 5.97662, 5.96846, 5.96044, 5.95256, 5.94481, 5.93719, 5.92971, 5.92235, 5.91513, 5.90804, 5.90107, 5.89424, 5.88753, 5.88095, 5.8745, 5.86817, 5.86197, 5.85589, 5.84993, 5.8441, 5.83839, 5.83281, 5.82734, 5.822, 5.81677, 5.81167, 5.80669, 5.80182, 5.79707, 5.79244, 5.78793, 5.78354, 5.77926, 5.7751, 5.77105, 5.76712, 5.7633, 5.7596, 5.75602, 5.93248, 6.12904, 6.24997, 6.24656, 6.24328, 6.24011, 6.23707, 6.23415, 6.23135, 6.22868, 6.22612, 6.22368, 6.22137, 6.21917, 6.2171, 6.21514, 6.2133, 6.21159, 6.20999, 6.20851, 6.20715, 6.20591, 6.20478, 6.20378, 6.20289, 6.20213, 6.20148, 6.20094, 6.20053, 6.20024, 6.20006, 6.2, 6.20006, 6.20024, 6.20053, 6.20094, 6.20148, 6.20213, 6.20289, 6.20378, 6.20478, 6.20591, 6.20715, 6.20851, 6.20999, 6.21159, 6.2133, 6.21514, 6.2171, 6.21917, 6.22137, 6.22368, 6.22612, 6.22868, 6.23135, 6.23415, 6.23707, 6.24011, 6.24328, 6.24656, 6.24997, 6.2535, 6.25715, 6.26093, 6.26483, 6.26886, 6.27301, 6.27728, 6.28168, 6.28621, 6.29086, 6.29564, 6.30055, 6.30559, 6.31075, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 10.1306, 10.1511, 10.1719, 10.1929, 10.2143, 10.2359, 10.2579, 10.2801, 10.3026, 10.3254, 10.3485, 10.3719, 10.3956, 10.4196, 10.444, 10.4686, 10.4936, 10.5189, 10.5445, 10.56, 10.4793, 10.4, 10.3221, 10.2455, 10.1703, 10.0963, 10.0236, 9.95218, 9.88191, 9.81282, 9.74486, 9.67803, 9.61228, 9.54761, 9.48398, 9.42137, 9.35975, 9.29912, 9.23944, 9.18069, 9.12286, 9.06592, 9.00986, 8.95466, 8.9003, 8.84677, 8.79404, 8.7421, 8.69093, 8.64053, 8.59087, 8.54193, 8.49372, 8.4462, 8.39937, 8.35322, 8.30772, 8.26288, 8.21868, 8.1751, 8.13213, 8.09353, 8.12334, 8.15354, 8.18412, 8.21508, 8.24644, 8.27819, 8.31035, 8.34292, 8.37591, 8.40932, 8.44316, 8.47743, 8.51215, 8.54731, 8.58293, 8.61901, 8.65557, 8.6926, 8.73011, 8.76812, 8.80663, 8.84565, 8.88519, 8.92525, 8.96585, 9.00699, 9.04868, 9.09093, 9.13376, 9.17716, 9.22116, 9.26575, 9.31096, 9.3568, 9.40326, 9.45037, 9.49813, 9.54657, 9.59568, 9.64549, 9.696, 9.74723, 9.79919, 9.8519, 9.90536, 9.95961, 10.0146, 10.0705, 10.1271, 10.1846, 10.243, 10.3022, 10.3623, 10.4233, 10.4852, 10.5481, 10.6119, 10.6767, 10.7425, 10.8094, 10.8773, 10.9462, 11.0163, 11.0874, 11.1597, 11.2332, 11.3078, 11.3837, 11.4608, 11.5392, 11.6189, 11.6999, 11.7823, 11.8661, 11.9513, 12.0379, 12.1261, 12.2158, 12.3071, 12.4, 5.41351, 5.40009, 5.38684, 5.37376, 5.36085, 5.3481, 5.33551, 5.32308, 5.31081, 5.2987, 5.28674, 5.27493, 5.26328, 5.25178, 5.24043, 5.22923, 5.21817, 5.20726, 5.1965, 5.18588, 5.1754, 5.16505, 5.15485, 5.14479, 5.13486, 5.12507, 5.11542, 5.10589, 5.0965, 5.08724, 5.07811, 5.06911, 5.06024, 5.0515, 5.04288, 5.03438, 5.02601, 5.01776, 5.00964, 5.00164, 4.99375, 4.98599, 4.97834, 4.97082, 4.96341, 4.95611, 4.94894, 4.94187, 4.93492, 4.92809, 4.92136, 4.91475, 4.90825, 4.90186, 4.89558, 4.88941, 4.88334, 4.87739, 4.87154, 4.8658, 4.86016, 4.85463, 4.84921, 4.84388, 4.83867, 4.83355, 4.82854, 4.82363, 4.81882, 4.81411, 4.80951, 4.805, 4.80059, 4.79629, 4.79208, 4.78797, 4.78396, 4.78004, 4.77623, 4.77251, 4.76888, 4.76535, 4.76192, 4.75859, 4.75535, 4.7522, 4.8783, 5.02971, 5.19092, 5.36291, 5.5468, 5.74386, 5.95555, 6.18357, 6.23707, 6.23415, 6.23135, 6.22868, 6.22612, 6.22368, 6.22137, 6.21917, 6.2171, 6.21514, 6.2133, 6.21159, 6.20999, 6.20851, 6.20715, 6.20591, 6.20478, 6.20378, 6.20289, 6.20213, 6.20148, 6.20094, 6.20053, 6.20024, 6.20006, 6.2, 6.20006, 6.20024, 6.20053, 6.20094, 6.20148, 6.20213, 6.20289, 6.20378, 6.20478, 6.20591, 6.20715, 6.20851, 6.20999, 6.21159, 6.2133, 6.21514, 6.2171, 6.21917, 6.22137, 6.22368, 6.22612, 6.22868, 6.23135, 6.23415, 6.23707, 6.24011, 6.24328, 6.24656, 6.24997, 6.2535, 6.25715, 6.26093, 6.26483, 6.26886, 6.27301, 6.27728, 6.28168, 6.28621, 6.29086, 6.29564, 6.30055, 6.30559, 6.31075, 6.31604, 6.32147, 6.32702, 6.3327, 6.33851, 6.34446, 6.35053, 6.35674, 6.36309, 6.36956, 6.37617, 6.38292, 6.3898, 6.39682, 6.40398, 6.41128, 6.41871, 6.42629, 6.434, 6.44186, 6.44986, 6.458, 6.46628, 6.47471, 6.48329, 6.49201, 6.50088, 6.5099, 6.51907, 6.52838, 6.53785, 6.54747, 6.55725, 6.56718, 6.57726, 6.5875, 6.5979, 6.60846, 6.61918, 6.63006, 6.6411, 6.6523, 6.66367, 6.67521, 6.68692, 6.69879, 6.71083, 6.72305, 6.73543, 6.748, 6.76073, 6.77365, 6.78674, 6.80002, 6.81348, 6.82712, 6.84094, 6.85496, 6.86916, 6.88355, 6.89813, 6.91291, 6.92788, 6.94305, 6.95842, 6.97399, 6.98977, 7.00575, 7.02193, 7.03833, 7.05494, 7.07176, 7.0888, 7.10605, 7.12352, 7.14122, 7.15914, 7.17729, 7.19567, 7.21428, 7.23313, 7.25221, 7.27153, 7.2911, 7.31091, 7.33096, 7.35127, 7.37183, 7.39265, 7.41373, 7.43507, 7.45668, 7.47855, 7.5007, 7.52312, 7.54582, 7.5688, 7.59207, 7.61563, 7.63948, 7.66362, 7.68807, 7.71282, 7.73787, 7.76324, 7.78893, 7.81493, 7.84035, 7.79649, 7.75327, 7.71066, 7.66867, 7.62728, 7.58647, 7.54624, 7.50658, 7.46747, 7.42891, 7.39089, 7.35339, 7.31641, 7.27994, 7.24397, 7.20849, 7.17349, 7.13896, 7.1049, 7.07129, 7.03814, 7.00543, 6.97315, 6.9413, 6.90987, 6.87886, 6.84825, 6.81804, 6.78823, 6.7588, 6.72975, 6.70108, 6.67279, 6.64485, 6.61727, 6.59005, 6.56317, 6.53664, 6.51044, 6.48458, 6.45904, 6.43382, 6.40892, 6.38434, 6.36006, 6.33609, 6.31242, 6.28904, 6.26595, 6.24316, 6.22064, 6.19841, 6.17645, 6.15476, 6.13334, 6.11218, 6.09129, 6.07065, 6.07791, 6.11273, 6.14807, 6.18393, 6.22034, 6.2573, 6.29482, 6.33291, 6.37159, 6.41087, 6.45075, 6.49126, 6.53241, 6.57421, 6.61668, 6.65982, 6.70366, 6.74821, 6.79349, 6.83951, 6.88629, 6.93384, 6.9822, 7.03136, 7.08136, 7.13221, 7.18393, 7.23655, 7.29009, 7.34456, 7.4, 7.45642, 7.51386, 7.57233, 7.63186, 7.69249, 7.75423, 7.81713, 7.8812, 7.94649, 8.01302, 8.08083, 8.14995, 8.22042, 8.29229, 8.36558, 8.44034, 8.51661, 8.59444, 8.62398, 8.60635, 8.58895, 8.57179, 8.55486, 8.53816, 8.52169, 8.50544, 8.48942, 8.47361, 8.45803, 8.44266, 8.42751, 8.41257, 8.39785, 8.38333, 8.36903, 8.35493, 8.34104, 8.32735, 8.31387, 8.30059, 8.2875, 8.27462, 8.26193, 8.24944, 8.23714, 8.22504, 8.21313, 8.20141, 8.18987, 8.17853, 8.16737, 8.1564, 8.14561, 8.135, 8.12458, 8.11434, 8.10427, 8.09439, 8.08468, 8.07515, 8.0658, 8.05662, 8.04762, 8.03879, 8.03013, 8.02164, 8.01332, 8.00517, 7.99719, 7.98938, 7.98174, 7.97426, 7.96694, 7.9598, 7.95281, 7.94599, 7.93933, 7.93284, 7.9265, 7.92033, 7.91431, 7.90846, 7.90276, 7.89723, 7.89185, 7.88663, 7.88156, 7.87666, 7.8719, 7.86731, 7.86286, 7.85858, 7.85444, 7.85046, 7.84664, 7.84296, 7.83944, 7.83608, 7.83286, 7.82979, 7.82688, 7.82412, 7.82151, 7.81905, 7.81674, 7.81458, 7.81257, 7.8107, 7.80899, 7.80743, 7.80602, 7.80475, 7.80364, 7.80267, 7.80186, 7.80119, 7.80067, 7.8003, 7.80007, 7.8, 7.80007, 7.8003, 7.80067, 7.80119, 7.80186, 7.80267, 7.80364, 7.80475, 7.80602, 7.80743, 7.80899, 7.8107, 7.81257, 7.81458, 7.81674, 7.81905, 7.82151, 7.82412, 7.82688, 7.82979, 7.83286, 7.83608, 7.83944, 7.84296, 7.84664, 7.85046, 7.85444, 7.85858, 7.86286, 7.86731, 7.8719, 7.87666, 7.88156, 7.88663 };
+double testRangesOrtho2[] = {8.82969, 8.82661, 8.8237, 8.82095, 8.81838, 8.81598, 8.81374, 8.81168, 8.80978, 8.80805, 8.80649, 8.8051, 8.80387, 8.80282, 8.80193, 8.80121, 8.80066, 8.80027, 8.80005, 8.8, 8.80012, 8.80041, 8.80086, 8.80148, 8.80227, 8.80322, 8.80434, 8.80564, 8.8071, 8.80872, 8.81052, 8.81248, 8.81462, 8.81692, 8.81939, 8.82203, 8.82484, 8.82782, 8.83097, 8.83429, 8.83778, 8.84145, 8.84528, 8.84929, 8.85347, 8.85782, 8.86234, 8.86704, 8.87191, 8.87696, 8.88218, 8.88758, 8.89315, 8.8989, 8.90482, 8.91093, 8.91721, 8.92367, 8.93031, 8.93713, 8.94413, 8.95132, 8.95868, 8.96623, 8.97396, 8.98188, 8.98998, 8.99827, 9.00674, 9.01541, 9.02426, 9.0333, 9.04253, 9.05195, 9.06157, 9.07138, 9.08138, 9.09158, 9.10197, 9.11256, 9.12336, 9.13435, 9.14554, 9.15693, 9.16853, 9.18033, 9.19233, 9.20455, 9.21697, 9.2296, 9.24244, 9.2555, 9.26876, 9.28225, 9.29595, 9.30986, 9.324, 9.33836, 9.35294, 9.36774, 9.38277, 9.39803, 9.41352, 9.42924, 9.44519, 9.46137, 9.4778, 9.49446, 9.51136, 9.5285, 9.54589, 9.56352, 9.5814, 9.59953, 9.61791, 9.63655, 9.65544, 9.67459, 9.69401, 9.71368, 9.73362, 9.75383, 9.77432, 9.79507, 9.8161, 9.83741, 9.859, 9.88087, 9.90303, 9.92547, 9.94821, 9.97125, 9.99458, 10.0182, 10.0421, 10.0664, 10.0909, 10.1158, 10.141, 10.1665, 10.1923, 10.2185, 10.2449, 10.2718, 10.2989, 10.3264, 10.3542, 10.3824, 10.411, 10.4399, 10.4691, 10.4988, 10.5287, 10.5591, 10.5898, 10.621, 10.6525, 10.6844, 10.7167, 10.7494, 10.7825, 10.816, 10.8499, 10.8843, 10.9191, 10.9543, 10.99, 11.026, 11.0626, 11.0996, 11.1371, 11.175, 11.2134, 11.2523, 11.2916, 11.3315, 11.3676, 11.3074, 11.2481, 11.1896, 11.1319, 11.075, 11.0189, 10.9636, 10.9091, 10.8553, 10.8022, 10.7498, 10.6982, 10.6472, 10.5969, 10.5473, 10.4984, 10.4501, 10.4025, 10.3555, 10.3091, 8.83781, 8.79889, 8.76048, 8.72257, 8.68515, 8.64822, 8.61176, 8.57577, 8.54024, 8.50517, 8.47054, 8.43635, 8.4026, 8.36928, 8.33638, 8.30389, 8.27181, 8.24013, 8.20886, 8.17797, 8.14747, 8.11735, 8.08761, 8.05823, 8.02922, 8.00057, 7.97228, 7.94433, 7.91674, 7.88948, 7.86256, 7.83597, 7.8097, 7.78376, 7.75814, 7.73284, 7.70784, 7.68315, 7.65877, 7.63468, 7.61089, 7.58739, 7.56418, 7.54126, 7.51861, 7.49625, 7.47415, 7.45233, 7.43078, 7.40949, 7.38847, 7.3677, 7.34719, 7.32693, 7.30692, 7.28716, 7.26765, 7.24837, 7.22934, 7.21054, 7.19198, 7.17364, 7.15554, 7.13766, 7.12001, 7.10258, 7.08537, 7.06838, 7.0516, 7.03503, 7.01868, 7.00254, 6.9866, 6.97086, 6.95533, 6.94, 6.92487, 6.90994, 6.8952, 6.88065, 6.8663, 6.85214, 6.83816, 6.82437, 6.81077, 6.79735, 6.78411, 6.77105, 6.75817, 6.74547, 6.73294, 6.72059, 6.70841, 6.6964, 6.68456, 6.67289, 6.66139, 6.65005, 6.63888, 6.62787, 6.61702, 6.60634, 6.59581, 6.58544, 6.57523, 6.56518, 6.55528, 6.54554, 6.53595, 6.52651, 6.51722, 6.50808, 6.49909, 6.49025, 6.48156, 6.47302, 6.46461, 6.45636, 6.44825, 6.44028, 6.43245, 6.42476, 6.41721, 6.40981, 6.40254, 6.39541, 6.38842, 6.38156, 6.37484, 6.36826, 6.36181, 6.35549, 6.34931, 6.34326, 6.33734, 6.33155, 6.3259, 6.32037, 6.31497, 6.30971, 6.30457, 6.29956, 6.29468, 6.28992, 6.2853, 6.28079, 6.27642, 6.27217, 6.26804, 6.26404, 6.26017, 6.25641, 6.25278, 6.24928, 6.24589, 6.24263, 6.23949, 6.23648, 6.23358, 6.23081, 6.22815, 6.22562, 6.22321, 6.22092, 6.21875, 6.2167, 6.21476, 6.21295, 6.21126, 6.20968, 6.20823, 6.20689, 6.20567, 6.20457, 6.20359, 6.20273, 6.20199, 6.20136, 6.20085, 6.20046, 6.20019, 6.20004, 6.2, 6.20008, 6.20029, 6.2006, 6.20104, 6.2016, 6.20227, 6.20306, 6.20397, 6.205, 6.20615, 6.20741, 6.20879, 6.2103, 6.21192, 6.21366, 6.21552, 6.2175, 6.2196, 6.22182, 6.22416, 6.22662, 6.2292, 6.2319, 6.23473, 6.23767, 6.24074, 6.24392, 6.24723, 6.25067, 6.25422, 6.2579, 6.2617, 6.26563, 6.26968, 6.27385, 6.27815, 6.28258, 6.28713, 6.29181, 6.29662, 6.30155, 6.30661, 6.3118, 6.31712, 6.32257, 6.32814, 6.33385, 6.33969, 6.34566, 6.35176, 6.358, 6.36437, 6.37087, 6.37751, 6.38429, 6.3912, 6.39824, 6.40543, 6.41275, 6.42022, 6.42782, 6.43556, 6.44345, 6.45147, 6.45964, 6.46796, 6.47642, 6.48502, 6.49377, 6.50267, 6.51172, 6.52092, 6.53027, 6.53976, 6.54942, 6.55922, 6.56918, 6.5793, 6.58957, 6.6, 6.61059, 6.62134, 6.63225, 6.64333, 6.65457, 6.66597, 6.67754, 6.68928, 6.70118, 6.71326, 6.72551, 6.73793, 6.75053, 6.7633, 6.77625, 6.78939, 6.8027, 6.81619, 6.82987, 6.84373, 6.85778, 6.87202, 6.88645, 6.90107, 6.91589, 6.9309, 6.94611, 6.96152, 6.97713, 6.99295, 7.00897, 7.0252, 7.04163, 7.05828, 7.07515, 7.09223, 7.10953, 7.12705, 7.14479, 7.16275, 7.18095, 7.19937, 7.21803, 7.23692, 7.25605, 7.27543, 7.29504, 7.3149, 7.33501, 7.35536, 7.37598, 7.39685, 7.41798, 7.43937, 7.46103, 7.48296, 7.50516, 7.52764, 7.55039, 7.57343, 7.59676, 7.62037, 7.64428, 7.66849, 7.69299, 7.7178, 7.74292, 7.76835, 7.75592, 7.71183, 7.66837, 7.62555, 7.58335, 7.54175, 7.50075, 7.46034, 7.4205, 7.38122, 7.34249, 7.30431, 7.26666, 7.22953, 7.19292, 7.15681, 7.12119, 7.08607, 7.05142, 7.01724, 6.98353, 6.95026, 6.91745, 6.88507, 6.85313, 6.82161, 6.7905, 6.75981, 6.72952, 6.69963, 6.67013, 6.64101, 6.61227, 6.58391, 6.55591, 6.52827, 6.50099, 6.47405, 6.44747, 6.42122, 6.3953, 6.36971, 6.34445, 6.31951, 6.29488, 6.27057, 6.24655, 6.22284, 6.19943, 6.17631, 6.15348, 6.13093, 6.10866, 6.08667, 6.06495, 6.0435, 6.02232, 6.0014, 5.98073, 5.96032, 5.94017, 5.92026, 5.90059, 5.88117, 5.86199, 5.84304, 5.82433, 5.80584, 5.78758, 5.76955, 5.75173, 5.73414, 5.71676, 5.69959, 5.68264, 5.66589, 5.64935, 5.63301, 5.61687, 5.60094, 5.58519, 5.56964, 5.55429, 5.53912, 5.52414, 5.50935, 5.49474, 5.48031, 5.46606, 5.45198, 5.43809, 5.42436, 5.41081, 5.39743, 5.38421, 5.37117, 5.35829, 5.34557, 5.33301, 5.32061, 5.30837, 5.29629, 5.28436, 5.27259, 5.26097, 5.2495, 5.23818, 5.22701, 5.21598, 5.2051, 5.19436, 5.18377, 5.17332, 5.163, 5.15283, 5.14279, 5.1329, 5.12313, 5.1135, 5.10401, 5.09464, 5.08541, 5.0763, 5.06733, 5.05848, 5.04976, 5.04117, 5.0327, 5.02435, 5.01613, 5.00803, 5.00005, 5.04308, 5.10535, 5.16927, 5.23491, 5.30234, 5.37164, 5.44288, 5.46606, 5.45839, 5.45085, 5.44344, 5.43615, 5.42898, 5.42193, 5.41501, 5.40821, 5.40152, 5.39496, 5.38851, 5.38218, 5.37597, 5.36987, 5.36389, 5.35803, 5.35228, 5.34664, 5.34112, 5.33571, 5.33041, 5.32523, 5.32015, 5.31519, 5.31033, 5.30559, 5.30096, 5.29643, 5.29201, 5.2877, 5.2835, 5.27941, 5.27542, 5.27154, 5.26776, 5.26409, 5.26053, 5.25707, 5.25371, 5.25046, 5.24731, 5.24427, 5.24133, 5.23849, 5.23576, 5.23312, 5.23059, 5.22817, 5.22584, 5.22361, 5.22149, 5.21947, 5.21754, 5.21572, 5.214, 5.21238, 5.21086, 5.20944, 5.20812, 5.2069, 5.20578, 5.20476, 5.20384, 5.20301, 5.20229, 5.20167, 5.20114, 5.20071, 5.20039, 5.20016, 5.20003, 5.2, 5.20007, 5.20024, 5.20051, 5.20087, 5.20134, 5.2019, 5.20257, 5.20333, 5.20419, 5.20515, 5.20622, 5.20738, 5.20864, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 8.84711, 8.86286, 8.87883, 8.89503, 8.91146, 8.92812, 8.94501, 8.96214, 8.97951, 8.99711, 9.01496, 9.03305, 9.05139, 9.06997, 9.0888, 9.10789, 9.12722, 9.14682, 9.16668, 9.18679, 9.20717, 9.22782, 9.24874, 9.21982, 9.14423, 9.07004, 8.99722, 8.92572, 8.85552, 8.78658, 8.71887, 8.65236, 8.58702, 8.52282, 8.45974, 8.39773, 8.33679, 8.27689, 8.21799, 8.16008, 8.10313, 8.04712, 7.99204, 7.93785, 7.88455, 7.8321, 7.78049, 7.7297, 7.67972, 7.63053, 7.5821, 7.53443, 7.48749, 7.44128, 7.39577, 7.35096, 7.30682, 7.26335, 7.22053, 7.17835, 7.13679, 7.09585, 7.0555, 7.01575, 6.97657, 6.93796, 6.8999, 6.86239, 6.82541, 6.78896, 6.75303, 6.76833, 6.79309, 6.81817, 6.84356, 6.86927, 6.8953, 6.92167, 6.94837, 6.97541, 7.0028, 7.03053, 7.05862, 7.08708, 7.1159, 7.14509, 7.17466, 7.20461, 7.23495, 7.26569, 7.29683, 7.32837, 7.36034, 7.39272, 7.42553, 7.45878, 7.49247, 7.52661, 7.5612, 7.59626, 7.6318, 7.66781, 7.70431, 7.74131, 7.77882, 7.81684, 7.85539, 7.89446, 7.93408, 7.97426, 8.01499, 8.0563, 8.09819, 8.14067, 8.18376, 8.22747, 8.2718, 8.31677, 8.36239, 8.40868, 8.45565, 8.5033, 8.55166, 8.60074, 8.65055, 8.70111, 8.75242, 8.80452, 8.85741, 8.91111, 8.96564, 9.02101, 9.07724, 9.13435, 9.19237, 9.2513, 9.31117, 9.372, 9.43381, 9.49662, 9.56046, 9.62535, 9.69131, 9.75836, 9.82654, 9.89587, 9.96638, 10.0381, 10.111, 10.1852, 10.2607, 10.3376, 10.4157, 10.4953, 10.5763, 10.6588, 10.7428, 10.8283, 10.9154, 11.0041, 11.0945, 11.1866, 11.2805, 11.3761, 11.4736, 11.5731, 11.6745, 11.7779, 11.8834, 11.991, 12.1008, 12.2129, 12.3273, 12.4441, 12.5635, 12.6853, 4.158, 4.15, 4.14211, 4.13432, 4.12664, 4.11907, 4.11161, 4.10425, 4.09699, 4.08984, 4.08279, 4.07584, 4.06899, 4.06224, 4.05559, 4.04904, 4.04259, 4.03624, 4.02998, 4.02382, 4.01775, 4.01178, 4.0059, 4.00012, 3.99443, 3.98883, 3.98332, 3.9779, 3.97257, 3.96733, 3.96218, 3.95712, 3.95215, 3.94727, 3.94247, 3.93776, 3.93313, 3.92859, 3.92414, 3.91977, 3.91548, 3.91128, 3.90716, 3.90312, 3.89917, 3.8953, 3.89151, 3.8878, 3.88418, 3.88063, 3.87716, 3.87378, 3.87047, 3.86724, 3.86409, 3.86102, 3.85803, 3.85511, 3.85228, 3.84952, 3.84684, 3.84423, 3.8417, 3.83925, 3.83688, 3.83458, 3.83235, 3.8302, 3.82813, 3.82613, 3.82421, 3.82236, 3.82058, 3.81888, 3.81726, 3.8157, 3.81423, 3.81282, 3.81149, 3.81023, 3.80905, 3.80794, 3.8069, 3.80593, 3.80504, 3.80422, 3.80348, 3.8028, 3.8022, 3.80167, 3.80122, 3.80083, 3.80052, 3.80028, 3.80012, 3.80002, 3.8, 3.80005, 3.80018, 3.80037, 3.80064, 3.80098, 3.80139, 3.80188, 3.80243, 3.80306, 3.80377, 3.80454, 3.80539, 3.80631, 3.80731, 3.80837, 3.80951, 3.81073, 5.31676, 5.31865, 5.32065, 5.32276, 5.32496, 5.32727, 5.32968, 5.3322, 5.33482, 5.33755, 5.34038, 5.34331, 5.34635, 5.34949, 5.35274, 5.3561, 5.35956, 5.36313, 5.36681, 5.37059, 5.37448, 5.37848, 5.38259, 5.38681, 5.39113, 5.39557, 5.40012, 5.40477, 5.40954, 5.41442, 5.41941, 5.42452, 5.42973, 5.43506, 5.44051, 5.44607, 5.45174, 5.45754, 5.46344, 5.46947, 5.47561, 5.48187, 5.48825, 5.49475, 5.50137, 5.50811, 5.51497, 5.52195, 5.52906, 5.53629, 5.54365, 5.55113, 5.55874, 5.56647, 5.57433, 5.58232, 5.59044, 5.59869, 5.60708, 5.61559, 5.62424, 5.63302, 5.64194, 5.65099, 5.66018, 5.66951, 5.67897, 5.68858, 5.69833, 5.70822, 5.71825, 5.72843, 5.73876, 5.74923, 5.75985, 5.77061, 5.78153, 5.79261, 5.80383, 5.81521, 5.82674, 5.83844, 5.85029, 5.8623, 5.87447, 5.8868, 5.8993, 5.91197, 5.9248, 5.9378, 5.95098, 5.96432, 5.97784, 5.99154, 6.00541, 6.01946, 6.0337, 6.04811, 6.06271, 6.0775, 6.09247, 6.10764, 6.123, 6.13855, 6.1543, 6.17025, 6.1864, 6.20276, 6.21932, 6.23608, 6.25306, 6.27025, 6.28765, 6.30527, 6.32311, 6.34117, 6.35946, 6.37798, 6.39672, 6.4157, 6.43492, 6.45437, 6.47406, 6.494, 6.51419, 6.53463, 6.55532, 6.57627, 6.59748, 6.61895, 6.64069, 6.6627, 6.68498, 6.70754, 6.73039, 6.75352, 6.77693, 6.80064, 6.82465, 6.84896, 6.87357, 6.89849, 6.92373, 6.94929, 6.97516, 7.00137, 7.0279, 7.05478, 7.08199, 7.10955, 7.13746, 7.16573, 7.19437, 7.22337, 7.25274, 7.28249, 7.31263, 7.34316, 7.37408, 7.40541, 7.43715, 7.4693, 7.50188, 7.53489, 7.56833, 7.60222, 7.63655, 7.67135, 7.70661, 7.74235, 7.77856, 7.81527, 7.85247, 7.82933, 7.79855, 7.76815, 7.73814, 7.70851, 7.67925, 7.65036, 7.62183, 7.59365, 7.56583, 7.53835, 7.51121, 7.48441, 7.45794, 7.4318, 7.40598, 7.38048, 7.35529, 7.33042, 7.30585, 7.28158, 7.25762, 7.23394, 7.21056, 7.18747, 7.16465, 7.14212, 7.11987, 7.09788, 7.07617, 7.05472, 7.03354, 7.01262, 6.99195, 6.97154, 6.95138, 6.93146, 6.91179, 6.89237, 6.87318, 6.85423, 6.83551, 6.81702, 6.79877, 6.78074, 6.76293, 6.74534, 6.72798, 6.71083, 6.69389, 6.67717, 6.66066, 6.64435, 6.62825, 6.61235, 6.59666, 6.58116, 6.56586, 6.55076, 6.53585, 6.52113, 6.5066, 6.49226, 6.47811, 6.46414, 6.45035, 6.45668, 6.51582, 6.57618, 6.63779, 6.7007, 6.76494, 6.83055, 6.89758, 6.96608, 7.03608, 7.10765, 7.18082, 7.25565, 7.3322, 7.41053, 7.49069, 7.57275, 7.65678, 7.74284, 7.831, 7.92136, 8.01397, 8.10893, 8.20633, 8.30626, 8.40881, 8.51409, 8.62221, 8.73327, 8.84741, 8.96474, 9.0854, 9.20952, 9.22452, 9.21197, 9.19964, 9.18751, 9.17558, 9.16386, 9.15235, 9.14104, 9.12993, 9.11902, 9.1083, 9.09779, 9.08748, 9.07736, 9.06743, 9.0577, 9.04816, 9.03882, 9.02966, 9.0207, 9.01192, 9.00333, 8.99493, 8.98672, 8.97869, 8.97085, 8.96319, 8.95571, 8.94842, 8.94131, 8.93438, 8.92763, 8.92107, 8.91468, 8.90847, 8.90243, 8.89658, 8.8909, 8.8854, 8.88007, 8.87492, 8.86994, 8.86514, 8.86051, 8.85606, 8.85177, 8.84766, 8.84373, 8.83996, 8.83637, 8.83294, 8.82969 };
+
diff --git a/test/testData.h b/test/testData.h
new file mode 100644
index 0000000000000000000000000000000000000000..3dcae10a8f87afbc5d18702509096731d75f28aa
--- /dev/null
+++ b/test/testData.h
@@ -0,0 +1,28 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+extern double testRanges[];
+
+extern double testRanges2[];
+
+extern double testRangesOrtho1[];
+
+extern double testRangesOrtho2[];
diff --git a/test/testFalkoAHT.cpp b/test/testFalkoAHT.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a392817300ae36a128d331e59c92c34afe772083
--- /dev/null
+++ b/test/testFalkoAHT.cpp
@@ -0,0 +1,110 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <iostream>
+
+
+#include <falkolib/Feature/FALKO.h>
+#include <falkolib/Feature/BSC.h>
+#include <falkolib/Feature/FALKOExtractor.h>
+
+#include <falkolib/Feature/BSCExtractor.h>
+
+#include <falkolib/Matching/NNMatcher.h>
+#include <falkolib/Matching/AHTMatcher.h>
+
+#include "testData.h"
+
+using namespace std;
+using namespace falkolib;
+
+int main(int argc, char** argv) {
+ LaserScan scan1(-0.003316126, 2.0 * M_PI, 1440);
+ scan1.fromRanges(testRanges);
+ LaserScan scan2(-0.003316126, 2.0 * M_PI, 1440);
+ scan2.fromRanges(testRanges2);
+
+ FALKOExtractor fe;
+ fe.setMinExtractionRange(0);
+ fe.setMaxExtractionRange(30);
+ fe.enableSubbeam(true);
+ fe.setNMSRadius(0.1);
+ fe.setNeighB(0.07);
+ fe.setBRatio(2.5);
+ fe.setGridSectors(16);
+
+ std::vector<FALKO> keypoints1;
+ std::vector<FALKO> keypoints2;
+
+
+ fe.extract(scan1, keypoints1);
+ fe.extract(scan2, keypoints2);
+
+ cout << "num keypoints1 extracted: " << keypoints1.size() << endl;
+ cout << "num keypoints2 extracted: " << keypoints2.size() << endl;
+
+ BSCExtractor<FALKO> bsc(16, 8);
+ vector<BSC> bscDesc1;
+ vector<BSC> bscDesc2;
+
+
+ bsc.compute(scan1, keypoints1, bscDesc1);
+ bsc.compute(scan2, keypoints2, bscDesc2);
+
+
+
+
+ NNMatcher<FALKO> matcher;
+ matcher.setDistanceThreshold(0.1);
+ std::vector<std::pair<int, int> > assoNN;
+ std::cout << "num matching NN: " << matcher.match(keypoints1, keypoints2, assoNN) << endl;
+ for (auto& match : assoNN) {
+ if (match.second >= 0) {
+ int i1 = match.first;
+ int i2 = match.second;
+ std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << endl;
+ }
+ }
+
+ cout << endl;
+ AHTMatcher<FALKO> aht(0.01, 0.01, 0.001, 0.2, 0.2, 0.05);
+ aht.setDistanceThreshold(0.1);
+ std::vector<std::pair<int, int> > assoAHT;
+ std::cout << "num matching AHT: " << aht.match(keypoints1, keypoints2, assoAHT) << endl;
+ for (auto& match : assoAHT) {
+ if (match.second >= 0) {
+ int i1 = match.first;
+ int i2 = match.second;
+ std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << endl;
+ }
+ }
+
+ Eigen::Affine2d transformNN;
+ Eigen::Affine2d transformAHT;
+ computeTransform(keypoints1, keypoints2, assoNN, transformNN);
+ computeTransform(keypoints1, keypoints2, assoAHT, transformAHT);
+
+ std::cout << "NN transform: " << std::endl;
+ std::cout << transformNN.inverse().matrix() << std::endl;
+ std::cout << "AHT transform: " << std::endl;
+ std::cout << transformAHT.inverse().matrix() << std::endl;
+
+ return 0;
+}
+
diff --git a/test/testFalkoCC.cpp b/test/testFalkoCC.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..63441839bfde80db5cd3ab14714e561d3496e280
--- /dev/null
+++ b/test/testFalkoCC.cpp
@@ -0,0 +1,111 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <iostream>
+
+
+#include <falkolib/Feature/FALKO.h>
+#include <falkolib/Feature/BSC.h>
+#include <falkolib/Feature/FALKOExtractor.h>
+
+#include <falkolib/Feature/BSCExtractor.h>
+
+#include <falkolib/Matching/NNMatcher.h>
+#include <falkolib/Matching/CCDAMatcher.h>
+
+#include "testData.h"
+
+using namespace std;
+using namespace falkolib;
+
+int main(int argc, char** argv) {
+ LaserScan scan1(-0.003316126, 2.0 * M_PI, 1440);
+ scan1.fromRanges(testRanges);
+ LaserScan scan2(-0.003316126, 2.0 * M_PI, 1440);
+ scan2.fromRanges(testRanges2);
+
+ FALKOExtractor fe;
+ fe.setMinExtractionRange(0);
+ fe.setMaxExtractionRange(30);
+ fe.enableSubbeam(true);
+ fe.setNMSRadius(0.1);
+ fe.setNeighB(0.07);
+ fe.setBRatio(2.5);
+ fe.setGridSectors(16);
+
+ std::vector<FALKO> keypoints1;
+ std::vector<FALKO> keypoints2;
+
+
+ fe.extract(scan1, keypoints1);
+ fe.extract(scan2, keypoints2);
+
+ cout << "num keypoints1 extracted: " << keypoints1.size() << endl;
+ cout << "num keypoints2 extracted: " << keypoints2.size() << endl;
+
+ BSCExtractor<FALKO> bsc(16, 8);
+ vector<BSC> bscDesc1;
+ vector<BSC> bscDesc2;
+
+
+ bsc.compute(scan1, keypoints1, bscDesc1);
+ bsc.compute(scan2, keypoints2, bscDesc2);
+
+
+
+
+ NNMatcher<FALKO> matcher;
+ matcher.setDistanceThreshold(0.1);
+ std::vector<std::pair<int, int> > assoNN;
+ std::cout << "num matching NN: " << matcher.match(keypoints1, keypoints2, assoNN) << endl;
+ for (auto& match : assoNN) {
+ if (match.second >= 0) {
+ int i1 = match.first;
+ int i2 = match.second;
+ std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << endl;
+ }
+ }
+
+ cout << endl;
+ CCDAMatcher<FALKO> cc;
+ cc.setDistMin(0.05);
+ cc.setDistTol(0.1);
+ std::vector<std::pair<int, int> > assoCC;
+ std::cout << "num matching CC: " << cc.match(keypoints1, keypoints2, assoCC) << endl;
+ for (auto& match : assoCC) {
+ if (match.second >= 0) {
+ int i1 = match.first;
+ int i2 = match.second;
+ std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << endl;
+ }
+ }
+
+// Eigen::Affine2d transformNN;
+// Eigen::Affine2d transformCC;
+// computeTransform(keypoints1, keypoints2, assoNN, transformNN);
+// computeTransform(keypoints1, keypoints2, assoCC, transformCC);
+//
+// std::cout << "NN transform: " << std::endl;
+// std::cout << transformNN.inverse().matrix() << std::endl;
+// std::cout << "AHT transform: " << std::endl;
+// std::cout << transformCC.inverse().matrix() << std::endl;
+
+ return 0;
+}
+
diff --git a/test/testKeypointFalko.cpp b/test/testKeypointFalko.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..71f7a961ec6470b8b47f7bfc539e57ba938c45b5
--- /dev/null
+++ b/test/testKeypointFalko.cpp
@@ -0,0 +1,147 @@
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <iostream>
+
+
+#include <falkolib/Feature/FALKO.h>
+#include <falkolib/Feature/CGH.h>
+#include <falkolib/Feature/BSC.h>
+#include <falkolib/Feature/FALKOExtractor.h>
+
+#include <falkolib/Feature/BSCExtractor.h>
+#include <falkolib/Feature/CGHExtractor.h>
+
+#include <falkolib/Matching/NNMatcher.h>
+#include <falkolib/Matching/AHTMatcher.h>
+
+#include "testData.h"
+
+using namespace std;
+using namespace falkolib;
+
+int main(int argc, char** argv) {
+ FALKOExtractor fe;
+ fe.setMinExtractionRange(1);
+ fe.setMaxExtractionRange(30);
+ fe.enableSubbeam(true);
+ fe.setNMSRadius(0.1);
+ fe.setNeighB(0.07);
+ fe.setBRatio(2.5);
+ fe.setGridSectors(16);
+
+ LaserScan scan1(-0.003316126, 2.0 * M_PI, 1440);
+ scan1.fromRanges(testRanges);
+ LaserScan scan2(-0.003316126, 2.0 * M_PI, 1440);
+ scan2.fromRanges(testRanges2);
+
+ std::vector<FALKO> keypoints1;
+ std::vector<FALKO> keypoints2;
+
+
+ fe.extract(scan1, keypoints1);
+ fe.extract(scan2, keypoints2);
+
+ cout << "num keypoints1 extracted: " << keypoints1.size() << endl;
+ cout << "num keypoints2 extracted: " << keypoints2.size() << endl;
+
+ CGHExtractor<FALKO> cgh(16);
+ BSCExtractor<FALKO> bsc(16, 8);
+
+ vector<CGH> cghDesc1;
+ vector<CGH> cghDesc2;
+ vector<BSC> bscDesc1;
+ vector<BSC> bscDesc2;
+
+
+ cgh.compute(scan1, keypoints1, cghDesc1);
+ bsc.compute(scan1, keypoints1, bscDesc1);
+ cgh.compute(scan2, keypoints2, cghDesc2);
+ bsc.compute(scan2, keypoints2, bscDesc2);
+
+
+
+ cout << endl;
+ NNMatcher<FALKO> matcher;
+ matcher.setDistanceThreshold(0.1);
+ std::vector<std::pair<int, int> > asso;
+ std::cout << "num matching NN: " << matcher.match(keypoints1, keypoints2, asso) << endl;
+ for (auto& match : asso) {
+ if (match.second >= 0) {
+ int i1 = match.first;
+ int i2 = match.second;
+ std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << "\t CHG Distance: " << (cghDesc1[i1].distance(cghDesc2[i2])) << "\t BSC Distance: " << (bscDesc1[i1].distance(bscDesc2[i2])) << endl;
+ }
+ }
+
+ cout << endl;
+ NNMatcher<FALKO, BSC> matcherFALKOBSC;
+ matcherFALKOBSC.setDistanceThreshold(0.1);
+ matcherFALKOBSC.setDescriptorThreshold(15);
+ std::cout << "num matching NN FALKO BSC: " << matcherFALKOBSC.match(keypoints1, bscDesc1, keypoints2, bscDesc2, asso) << endl;
+ for (auto& match : asso) {
+ if (match.second >= 0) {
+ int i1 = match.first;
+ int i2 = match.second;
+ std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << "\t CHG Distance: " << (cghDesc1[i1].distance(cghDesc2[i2])) << "\t BSC Distance: " << (bscDesc1[i1].distance(bscDesc2[i2])) << endl;
+ }
+ }
+
+ cout << endl;
+ NNMatcher<FALKO, CGH> matcherFALKOCGH;
+ matcherFALKOCGH.setDistanceThreshold(0.1);
+ matcherFALKOCGH.setDescriptorThreshold(0.2);
+ std::cout << "num matching NN FALKO CGH: " << matcherFALKOCGH.match(keypoints1, cghDesc1, keypoints2, cghDesc2, asso) << endl;
+ for (auto& match : asso) {
+ if (match.second >= 0) {
+ int i1 = match.first;
+ int i2 = match.second;
+ std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << "\t CHG Distance: " << (cghDesc1[i1].distance(cghDesc2[i2])) << "\t BSC Distance: " << (bscDesc1[i1].distance(bscDesc2[i2])) << endl;
+ }
+ }
+
+ cout << endl;
+ NNMatcher<BSC> matcherBSC;
+ matcherBSC.setDistanceThreshold(15);
+ std::cout << "num matching NN BSC: " << matcherBSC.match(bscDesc1, bscDesc2, asso) << endl;
+ for (auto& match : asso) {
+ if (match.second >= 0) {
+ int i1 = match.first;
+ int i2 = match.second;
+ std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << "\t CHG Distance: " << (cghDesc1[i1].distance(cghDesc2[i2])) << "\t BSC Distance: " << (bscDesc1[i1].distance(bscDesc2[i2])) << endl;
+ }
+ }
+ cout << endl;
+ NNMatcher<CGH> matcherCGH;
+ matcherCGH.setDistanceThreshold(0.2);
+ std::cout << "num matching NN CGH: " << matcherCGH.match(cghDesc1, cghDesc2, asso) << endl;
+ for (auto& match : asso) {
+ if (match.second >= 0) {
+ int i1 = match.first;
+ int i2 = match.second;
+ std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << "\t CHG Distance: " << (cghDesc1[i1].distance(cghDesc2[i2])) << "\t BSC Distance: " << (bscDesc1[i1].distance(bscDesc2[i2])) << endl;
+ }
+ }
+
+ AHTMatcher<FALKO> aht;
+
+
+ return 0;
+}
+
diff --git a/test/testKeypointOC.cpp b/test/testKeypointOC.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6ca64080653ab34ca2770bc8de0a30017b8bf782
--- /dev/null
+++ b/test/testKeypointOC.cpp
@@ -0,0 +1,105 @@
+#include <iostream>
+/**
+ * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant
+ * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini.
+ *
+ * This file is part of FALKOLib.
+ *
+ * FALKOLib is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * FALKOLib 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 Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with FALKOLib. If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <falkolib/Feature/OC.h>
+#include <falkolib/Feature/CGH.h>
+#include <falkolib/Feature/BSC.h>
+
+#include <falkolib/Feature/OCExtractor.h>
+#include <falkolib/Feature/BSCExtractor.h>
+#include <falkolib/Feature/CGHExtractor.h>
+
+#include <falkolib/Matching/NNMatcher.h>
+#include <falkolib/Matching/CCDAMatcher.h>
+
+#include "testData.h"
+
+//#ifdef FEATURELIB_DEBUG
+//#include <gnuplot-iostream.h>
+//#endif
+
+using namespace std;
+using namespace falkolib;
+
+int main(int argc, char** argv) {
+ OCExtractor oe;
+ oe.setAngleRes(M_PI / 180.0 * 0.25);
+ oe.setTol(0.1);
+ oe.setRangeRes(0.05);
+ oe.setRangeMax(10.0);
+ oe.setNMSRadius(0.3);
+ oe.setNeighMinPoint(2);
+
+ LaserScan scan1(-0.003316126, 2.0 * M_PI, 1440);
+ scan1.fromRanges(testRangesOrtho1);
+ LaserScan scan2(-0.003316126, 2.0 * M_PI, 1440);
+ scan2.fromRanges(testRangesOrtho2);
+
+ std::vector<OC> keypoints1;
+ std::vector<OC> keypoints2;
+
+ std::cout << "extract OC keypoints: " << std::endl;
+ oe.extract(scan1, keypoints1);
+ oe.extract(scan2, keypoints2);
+
+ cout << "num keypoints1 extracted: " << keypoints1.size() << endl;
+ for (auto& kp : keypoints1) {
+ std::cout << kp.point.transpose() << ", index " << kp.index << ", radius " << kp.radius << ", orientation [deg] " << (180.0 / M_PI * kp.orientation) << std::endl;
+ }
+
+ cout << "num keypoints2 extracted: " << keypoints2.size() << endl;
+ for (auto& kp : keypoints2) {
+ std::cout << kp.point.transpose() << ", index " << kp.index << ", radius " << kp.radius << ", orientation [deg] " << (180.0 / M_PI * kp.orientation) << std::endl;
+ }
+
+//#ifdef FEATURELIB_DEBUG
+// Gnuplot gp("gnuplot -persist");
+// gp << "set size ratio -1\n";
+// for (int i = 0; i < scan1.points.size(); i += 10) {
+// gp << "set label \"" << i << "\" at " << scan1.points[i].x() << "," << scan1.points[i].y() << "\n";
+// }
+// gp << "plot '-' u 1:2 notitle pt 7 w p, '-' u 1:2 notitle pt 7 w p\n";
+// for (auto& p : scan1.points) {
+// gp << p.x() << " " << p.y() << "\n";
+// }
+// gp << "e" << std::endl;
+// for (auto& kp : keypoints1) {
+// gp << kp.point.x() << " " << kp.point.y() << "\n";
+// }
+// gp << "e" << std::endl;
+//#endif
+
+ CGHExtractor<OC> cgh(16);
+ BSCExtractor<OC> bsc(16, 8);
+
+ vector<CGH> cghDesc1;
+ vector<CGH> cghDesc2;
+ vector<BSC> bscDesc1;
+ vector<BSC> bscDesc2;
+
+
+ cgh.compute(scan1, keypoints1, cghDesc1);
+ bsc.compute(scan1, keypoints1, bscDesc1);
+ cgh.compute(scan2, keypoints2, cghDesc2);
+ bsc.compute(scan2, keypoints2, bscDesc2);
+
+ return 0;
+}
+